├── .gitignore ├── LICENSE ├── README.md ├── config.yml ├── demo ├── demo_actuator.py ├── demo_keyboard_controller.py ├── demo_mqtt_receiver.py └── demo_mqtt_sender.py ├── parts └── actuator.py ├── pc_main.py ├── pi_main.py ├── requirements.txt ├── tests ├── setup.py └── test_actuator.py └── util └── data.py /.gitignore: -------------------------------------------------------------------------------- 1 | .* 2 | !/.gitignore 3 | 4 | .ipynb_checkpoints/ 5 | */.ipynb_checkpoints/* 6 | 7 | __pycache__/ 8 | */__pycache__/* 9 | 10 | config.yml 11 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RCVehiclePy 2 | This is a Tool to Control steering and throttle of a R/C car via MQTT with Raspberry Pi 3B. 3 | 4 | [![demo_off_road](https://raw.githubusercontent.com/wiki/shirokunet/RCVehiclePy/images/off_road.gif)](https://www.youtube.com/watch?v=wb1s7gsLcVM) 5 | 6 | 7 | ## Table of Contents 8 | * [Prepare Hardware](#Prepare-Hardware) 9 | * [Getting Started](#Getting-Started) 10 | * [Demo](#Demo) 11 | * [Acknowledgments](#Acknowledgments) 12 | * [License](#License) 13 | 14 | 15 | ## Prepare Hardware 16 | ### Setup Raspberry Pi 3B 17 | - You can read my simple instruction at [here](https://shiroku.net/robotics/raspberry-pi-3b-setup/). 18 | 19 | ### Build a car 20 | 21 | 22 | - Hardware is almost same as a [Donkey Car](http://docs.donkeycar.com/guide/build_hardware/). 23 | - I bought below parts. The cost is about $130. 24 | * $84.95 [Exceed Racing Desert Short Course Truck 1/16 Scale Ready to Run 2.4ghz (AA Blue)](https://www.amazon.com/dp/9269802086/) 25 | * $28.84 [Raspberry Pi Camera Module V2-8 Megapixel,1080p](https://www.amazon.com/dp/B01ER2SKFS/) 26 | * $9.99 [SunFounder PCA9685 16 Channel 12 Bit PWM Servo Driver for Arduino and Raspberry Pi](https://www.amazon.com/dp/B014KTSMLA/) 27 | * $6.98 [Maxmoral 2 Set (40P/Set) 10cm Female to Female Jumper Wire](https://www.amazon.com/dp/B010L30SE8/) 28 | 29 | ### Access point 30 | We have several ways to connect Host PC and Raspberry Pi in outside. 31 | - Setting Raspberry Pi as an access point. [(documentation)](https://www.raspberrypi.org/documentation/configuration/wireless/access-point.md) 32 | - Put a smart phone on a R/C car, and tethering both of them. 33 | - Put a mobile wifi router on a R/C car. 34 | 35 | 36 | ## Getting Started 37 | ### Connect to Pi via SSH 38 | After setup access points, 39 | ``` 40 | $ sudo ssh pi@raspberrypi.local 41 | ``` 42 | 43 | ### Setup momo 44 | Momo is a powerfuil WebRTC Native Client. You can follow my instraction at [here](https://shiroku.net/robotics/run-webrtc-native-client-momo-on-raspberry-pi-3b/). 45 | 46 | ### Clone Repository 47 | ``` 48 | $ cd ~/ 49 | $ git clone https://github.com/shirokunet/RCVehiclePy 50 | ``` 51 | 52 | ### Update pip3 (optional) 53 | ``` 54 | $ sudo apt-get remove python-pip python3-pip 55 | $ wget https://bootstrap.pypa.io/get-pip.py 56 | $ sudo python3 get-pip.py 57 | ``` 58 | 59 | ### Setup Prerequisites 60 | - Python 3.5.x 61 | - numpy 1.16.1 62 | - PyYAML 3.12 63 | 64 | ``` 65 | $ cd ~/RCVehiclePy/ 66 | $ sudo pip3 install -r requirements.txt 67 | $ sudo apt-get install python3-numpy 68 | $ sudo apt-get install python3-yaml 69 | ``` 70 | 71 | ### Check Pi's IP address 72 | ``` 73 | $ ifconfig 74 | wlan0: flags=4163 mtu 1500 75 | inet xx.xx.xx.xx netmask 255.255.255.0 76 | ``` 77 | 78 | ### Setup config file 79 | Then chenge config file 80 | ``` 81 | $ cd ~/RCVehiclePy/ 82 | $ vi config.yml 83 | pi_ip: 'xx.xx.xx.xx' 84 | ``` 85 | 86 | ### Lunch MQTT publisher on the Raspberry Pi 87 | 88 | ``` 89 | $ cd ~/RCVehiclePy/ 90 | $ python3 pi_main.py 91 | ``` 92 | 93 | ### Set environment on the host PC 94 | Same as the step of the Raspberry Pi. 95 | - [Clone Repository](#clone-repository) 96 | - [Setup Prerequisites](#setup-prerequisites) 97 | - [Setup config file](#setup-config-file) 98 | 99 | ### Lunch MQTT publisher on the host PC 100 | 101 | ``` 102 | $ cd ~/RCVehiclePy/ 103 | $ python3 pc_main.py 104 | ``` 105 | 106 | ### Send command to Pi from the host PC 107 | Just push key "UP" or "DOWN" or "LEFT" or "RIGHT" on the terminal. 108 | 109 | ### Stop Program 110 | Type 'Ctrl' + 'c' in both terminals. 111 | 112 | 113 | ## Demo 114 | The latency is about 100~300msec. It can be controlled easily even if you watch only the camera streaming. 115 | [![demo_flat_road](https://raw.githubusercontent.com/wiki/shirokunet/RCVehiclePy/images/flat_road.gif)](https://www.youtube.com/watch?v=Y4eQZay4Up8) 116 | 117 | [![camera_flat_road](https://raw.githubusercontent.com/wiki/shirokunet/RCVehiclePy/images/flat_road_cam.gif)](https://www.youtube.com/watch?v=cbQFkdlA74Y) 118 | 119 | 120 | ## Acknowledgments 121 | 122 | * [Actuator layer](https://github.com/shirokunet/RCVehiclePy/blob/master/parts/actuator.py) is built upon [autorope/donkeycar](https://github.com/autorope/donkeycar). Thank you! 123 | 124 | 125 | ## License 126 | 127 | This project is licensed under the Apache License 2.0 - see the [LICENSE](LICENSE) file for details 128 | 129 | -------------------------------------------------------------------------------- /config.yml: -------------------------------------------------------------------------------- 1 | --- 2 | pi_ip: '192.168.43.5' 3 | #pi_ip: '107.242.121.2' 4 | -------------------------------------------------------------------------------- /demo/demo_actuator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import numpy as np 5 | import time 6 | 7 | import sys, os 8 | sys.path.append(os.path.join(os.path.dirname('__file__'), '..')) 9 | from parts.actuator import PCA9685, PWMSteering, PWMThrottle 10 | 11 | 12 | class ControlTest(): 13 | def __init__(self, throttle_id=0, steering_id=1): 14 | self.throt = PWMThrottle(PCA9685(throttle_id)) 15 | self.steer = PWMSteering(PCA9685(steering_id)) 16 | self.throt_test_rate = 0.3 17 | self.steer_test_rate = 1.0 18 | self.throt_direction = -1 19 | self.steer_direction = -1 20 | self.loop_interval = 0.05 21 | return 22 | 23 | def test_loop(self, mode='steer'): 24 | if mode == 'throt': 25 | controller = self.throt 26 | rate = self.throt_test_rate 27 | direction = self.throt_direction 28 | test_array = np.concatenate([np.arange(0.0, 1.0, 0.01), \ 29 | np.arange(1.0, 0.0, -0.01)]) 30 | elif mode == 'steer': 31 | controller = self.steer 32 | rate = self.steer_test_rate 33 | direction = self.steer_direction 34 | test_array = np.concatenate([np.arange(0.0, -1.0, -0.01), \ 35 | np.arange(-1.0, 1.0, 0.01), \ 36 | np.arange(1.0, 0.0, -0.01)]) 37 | 38 | for i in range(len(test_array)): 39 | value = test_array[i] * rate 40 | controller.run(value * direction) 41 | print(mode, value) 42 | time.sleep(self.loop_interval) 43 | 44 | controller.run(0) 45 | 46 | return 47 | 48 | def shutdown(self): 49 | self.throt.shutdown() 50 | self.steer.shutdown() 51 | return 52 | 53 | 54 | def main(): 55 | ct = ControlTest() 56 | ct.test_loop(mode='throt') 57 | ct.test_loop(mode='steer') 58 | ct.shutdown() 59 | return 60 | 61 | 62 | if __name__ == '__main__': 63 | main() 64 | -------------------------------------------------------------------------------- /demo/demo_keyboard_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import ctypes 5 | from multiprocessing import Process, Value 6 | from pynput import keyboard 7 | from time import sleep 8 | 9 | import sys, os 10 | sys.path.append(os.path.join(os.path.dirname('__file__'), '..')) 11 | from parts.actuator import PCA9685, PWMSteering, PWMThrottle 12 | 13 | 14 | class MPDriverInput(): 15 | def __init__(self): 16 | self.throttle = Value(ctypes.c_float,0.0) 17 | self.steering = Value(ctypes.c_float,0.0) 18 | self.is_run = Value(ctypes.c_bool,True) 19 | 20 | self._p = Process(target=self._process, args=(10,)) 21 | self._p.start() 22 | return 23 | 24 | def close(self): 25 | self.is_run.value = False 26 | self._p.join() 27 | 28 | def _on_press(self, key): 29 | if key == keyboard.Key.esc: 30 | self.is_run.value = False 31 | elif key == keyboard.Key.up: 32 | self.throttle.value = 1.0 33 | elif key == keyboard.Key.down: 34 | self.throttle.value = -1.0 35 | elif key == keyboard.Key.left: 36 | self.steering.value = -1.0 37 | elif key == keyboard.Key.right: 38 | self.steering.value = 1.0 39 | 40 | def _on_release(self, key): 41 | if key == keyboard.Key.up or key == keyboard.Key.down: 42 | self.throttle.value = 0.0 43 | elif key == keyboard.Key.left or key == keyboard.Key.right: 44 | self.steering.value = 0.0 45 | 46 | def _process(self, num): 47 | key_listener = keyboard.Listener(on_press=self._on_press, \ 48 | on_release=self._on_release) 49 | key_listener.start() 50 | while self.is_run.value: 51 | sleep(0.01) 52 | key_listener.stop() 53 | 54 | 55 | class RCController(): 56 | def __init__(self, throttle_id=0, steering_id=1): 57 | self.throt = PWMThrottle(PCA9685(throttle_id)) 58 | self.steer = PWMSteering(PCA9685(steering_id)) 59 | self.throt_test_rate = 0.3 60 | self.steer_test_rate = 1.0 61 | self.throt_direction = -1 62 | self.steer_direction = -1 63 | return 64 | 65 | def set_value(self, value, mode='steer'): 66 | if mode == 'throt': 67 | controller = self.throt 68 | rate = self.throt_test_rate 69 | direction = self.throt_direction 70 | elif mode == 'steer': 71 | controller = self.steer 72 | rate = self.steer_test_rate 73 | direction = self.steer_direction 74 | value = value * rate 75 | controller.run(value * direction) 76 | return 77 | 78 | def shutdown(self): 79 | self.throt.shutdown() 80 | self.steer.shutdown() 81 | return 82 | 83 | 84 | def main(): 85 | mp_driver = MPDriverInput() 86 | # controller = RCController() 87 | 88 | print('Press esc to exit') 89 | 90 | while mp_driver.is_run.value: 91 | # controller.set_value(mp_driver.throttle.value, mode='throt') 92 | # controller.set_value(mp_driver.steering.value, mode='steer') 93 | 94 | print('Throttle: {}, Steering: {}'.format(mp_driver.throttle.value, mp_driver.steering.value)) 95 | sleep(0.01) 96 | 97 | mp_driver.close() 98 | 99 | 100 | if __name__ == '__main__': 101 | main() 102 | -------------------------------------------------------------------------------- /demo/demo_mqtt_receiver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import paho.mqtt.client as mqtt 5 | 6 | host = '10.0.0.88' 7 | port = 1883 8 | keepalive = 60 9 | topic = 'mqtt/sensor' 10 | 11 | def on_connect(client, userdata, flags, rc): 12 | print('Connected with result code ' + str(rc)) 13 | client.subscribe(topic) 14 | 15 | def on_message(client, userdata, msg): 16 | print(msg.topic + ' ' + str(msg.payload)) 17 | 18 | client = mqtt.Client() 19 | client.on_connect = on_connect 20 | client.on_message = on_message 21 | 22 | client.connect(host, port, keepalive) 23 | 24 | client.loop_forever() 25 | -------------------------------------------------------------------------------- /demo/demo_mqtt_sender.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import time 5 | import paho.mqtt.client as mqtt 6 | 7 | host = '10.0.0.88' 8 | port = 1883 9 | keepalive = 60 10 | topic = 'mqtt/sensor' 11 | 12 | client = mqtt.Client() 13 | client.connect(host, port, keepalive) 14 | 15 | 16 | while True: 17 | client.publish(topic, 'Hello') 18 | time.sleep(1) 19 | 20 | client.disconnect() 21 | -------------------------------------------------------------------------------- /parts/actuator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | actuators.py 5 | Classes to control the motors and servos. These classes 6 | are wrapped in a mixer class before being used in the drive loop. 7 | """ 8 | 9 | import time 10 | 11 | import sys, os 12 | sys.path.append(os.path.join(os.path.dirname('__file__'), '../')) 13 | from util import data 14 | 15 | 16 | class PCA9685: 17 | """ 18 | PWM motor controler using PCA9685 boards. 19 | This is used for most RC Cars 20 | """ 21 | def __init__(self, channel, frequency=60): 22 | import Adafruit_PCA9685 23 | # Initialise the PCA9685 using the default address (0x40). 24 | self.pwm = Adafruit_PCA9685.PCA9685() 25 | self.pwm.set_pwm_freq(frequency) 26 | self.channel = channel 27 | 28 | def set_pulse(self, pulse): 29 | try: 30 | self.pwm.set_pwm(self.channel, 0, pulse) 31 | except OSError as err: 32 | print("Unexpected issue setting PWM (check wires to motor board): {0}".format(err)) 33 | 34 | def run(self, pulse): 35 | self.set_pulse(pulse) 36 | 37 | 38 | class PWMSteering: 39 | """ 40 | Wrapper over a PWM motor cotnroller to convert angles to PWM pulses. 41 | """ 42 | LEFT_ANGLE = -1 43 | RIGHT_ANGLE = 1 44 | 45 | def __init__(self, controller=None, 46 | left_pulse=290, right_pulse=490): 47 | 48 | self.controller = controller 49 | self.left_pulse = left_pulse 50 | self.right_pulse = right_pulse 51 | 52 | def run(self, angle): 53 | # map absolute angle to angle that vehicle can implement. 54 | pulse = data.map_range( 55 | angle, 56 | self.LEFT_ANGLE, self.RIGHT_ANGLE, 57 | self.left_pulse, self.right_pulse 58 | ) 59 | 60 | self.controller.set_pulse(pulse) 61 | 62 | def shutdown(self): 63 | self.run(0) # set steering straight 64 | 65 | 66 | class PWMThrottle: 67 | """ 68 | Wrapper over a PWM motor cotnroller to convert -1 to 1 throttle 69 | values to PWM pulses. 70 | """ 71 | MIN_THROTTLE = -1 72 | MAX_THROTTLE = 1 73 | 74 | def __init__(self, 75 | controller=None, 76 | max_pulse=300, 77 | min_pulse=490, 78 | zero_pulse=350): 79 | 80 | self.controller = controller 81 | self.max_pulse = max_pulse 82 | self.min_pulse = min_pulse 83 | self.zero_pulse = zero_pulse 84 | 85 | # send zero pulse to calibrate ESC 86 | self.controller.set_pulse(self.zero_pulse) 87 | time.sleep(1) 88 | 89 | def run(self, throttle): 90 | if throttle > 0: 91 | pulse = data.map_range(throttle, \ 92 | 0, self.MAX_THROTTLE, \ 93 | self.zero_pulse, self.max_pulse) 94 | else: 95 | pulse = data.map_range(throttle, \ 96 | self.MIN_THROTTLE, 0, \ 97 | self.min_pulse, self.zero_pulse) 98 | 99 | self.controller.set_pulse(pulse) 100 | 101 | def shutdown(self): 102 | self.run(0) # stop vehicle 103 | 104 | 105 | class Adafruit_DCMotor_Hat: 106 | """ 107 | Adafruit DC Motor Controller 108 | Used for each motor on a differential drive car. 109 | """ 110 | def __init__(self, motor_num): 111 | from Adafruit_MotorHAT import Adafruit_MotorHAT 112 | import atexit 113 | 114 | self.FORWARD = Adafruit_MotorHAT.FORWARD 115 | self.BACKWARD = Adafruit_MotorHAT.BACKWARD 116 | self.mh = Adafruit_MotorHAT(addr=0x60) 117 | 118 | self.motor = self.mh.getMotor(motor_num) 119 | self.motor_num = motor_num 120 | 121 | atexit.register(self.turn_off_motors) 122 | self.speed = 0 123 | self.throttle = 0 124 | 125 | def run(self, speed): 126 | """ 127 | Update the speed of the motor where 1 is full forward and 128 | -1 is full backwards. 129 | """ 130 | if speed > 1 or speed < -1: 131 | raise ValueError("Speed must be between 1(forward) and -1(reverse)") 132 | 133 | self.speed = speed 134 | self.throttle = int(data.map_range(abs(speed), -1, 1, -255, 255)) 135 | 136 | if speed > 0: 137 | self.motor.run(self.FORWARD) 138 | else: 139 | self.motor.run(self.BACKWARD) 140 | 141 | self.motor.setSpeed(self.throttle) 142 | 143 | def shutdown(self): 144 | self.mh.getMotor(self.motor_num).run(Adafruit_MotorHAT.RELEASE) 145 | -------------------------------------------------------------------------------- /pc_main.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import ctypes 5 | import json 6 | import paho.mqtt.client as mqtt 7 | import time 8 | import yaml 9 | from multiprocessing import Process, Value 10 | from pynput import keyboard 11 | 12 | 13 | class MQTTSender(): 14 | def __init__(self, host='localhost', port=1883, keepalive=60): 15 | self._client = mqtt.Client() 16 | self._client.connect(host, port, keepalive) 17 | return 18 | 19 | def disconnect(self): 20 | self._client.disconnect() 21 | return 22 | 23 | def publish(self, driver_msg, topic='mqtt/sensor'): 24 | self._client.publish(topic, driver_msg) 25 | return 26 | 27 | 28 | class MPDriverInput(): 29 | def __init__(self): 30 | self.throttle = Value(ctypes.c_float, 0.0) 31 | self.steering = Value(ctypes.c_float, 0.0) 32 | self.is_run = Value(ctypes.c_bool, True) 33 | 34 | self._p = Process(target=self._process, args=()) 35 | self._p.start() 36 | return 37 | 38 | def close(self): 39 | self.is_run.value = False 40 | self._p.join() 41 | 42 | def _on_press(self, key): 43 | if key == keyboard.Key.esc: 44 | self.is_run.value = False 45 | elif key == keyboard.Key.up: 46 | self.throttle.value = 1.0 47 | elif key == keyboard.Key.down: 48 | self.throttle.value = -1.0 49 | elif key == keyboard.Key.left: 50 | self.steering.value = -1.0 51 | elif key == keyboard.Key.right: 52 | self.steering.value = 1.0 53 | 54 | def _on_release(self, key): 55 | if key == keyboard.Key.up or key == keyboard.Key.down: 56 | self.throttle.value = 0.0 57 | elif key == keyboard.Key.left or key == keyboard.Key.right: 58 | self.steering.value = 0.0 59 | 60 | def _process(self): 61 | key_listener = keyboard.Listener(on_press=self._on_press, \ 62 | on_release=self._on_release) 63 | key_listener.start() 64 | while self.is_run.value: 65 | time.sleep(0.01) 66 | key_listener.stop() 67 | 68 | 69 | def main(): 70 | ymlfile = open('config.yml') 71 | cfg = yaml.load(ymlfile) 72 | ymlfile.close() 73 | 74 | sender = MQTTSender(host=cfg['pi_ip']) 75 | mp_driver = MPDriverInput() 76 | 77 | driver_msg = { 78 | 'time': time.time(), 79 | 'throttle': 0.0, 80 | 'steering': 0.0 81 | } 82 | 83 | print('Press esc to exit:') 84 | while mp_driver.is_run.value: 85 | driver_msg['time'] = int(time.time()) 86 | driver_msg['throttle'] = mp_driver.throttle.value 87 | driver_msg['steering'] = mp_driver.steering.value 88 | sender.publish(json.dumps(driver_msg)) 89 | print('Time:{}, Throttle: {}, Steering: {}'.format(driver_msg['time'], driver_msg['throttle'], driver_msg['steering'])) 90 | time.sleep(0.01) 91 | 92 | mp_driver.close() 93 | sender.disconnect() 94 | 95 | 96 | if __name__ == '__main__': 97 | main() 98 | -------------------------------------------------------------------------------- /pi_main.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import ctypes 5 | import json 6 | import paho.mqtt.client as mqtt 7 | import time 8 | import yaml 9 | import numpy as np 10 | from multiprocessing import Process, Value 11 | from parts.actuator import PCA9685, PWMSteering, PWMThrottle 12 | 13 | 14 | class MPMQTTReceiver(): 15 | def __init__(self, host='localhost', port=1883, keepalive=60): 16 | self.time = Value(ctypes.c_int, 0) 17 | self.throttle = Value(ctypes.c_float, 0.0) 18 | self.steering = Value(ctypes.c_float, 0.0) 19 | self.is_run = Value(ctypes.c_bool, True) 20 | 21 | self._client = mqtt.Client() 22 | self._client.on_connect = self._on_connect 23 | self._client.on_message = self._on_message 24 | self._client.connect(host, port, keepalive) 25 | 26 | self._p = Process(target=self._process, args=()) 27 | self._p.start() 28 | return 29 | 30 | def close(self): 31 | self.is_run.value = False 32 | self._p.join() 33 | 34 | def _on_connect(self, client, userdata, flags, rc, topic='mqtt/sensor'): 35 | self._client.subscribe(topic) 36 | 37 | def _on_message(self, client, userdata, msg): 38 | driver_msg = json.loads(msg.payload.decode('utf-8')) 39 | self.time.value = driver_msg['time'] 40 | self.throttle.value = driver_msg['throttle'] 41 | self.steering.value = driver_msg['steering'] 42 | 43 | def _process(self): 44 | self._client.loop_start() 45 | while self.is_run.value: 46 | time.sleep(0.01) 47 | self._client.loop_stop() 48 | 49 | 50 | class RCController(): 51 | def __init__(self, throttle_id=0, steering_id=1): 52 | self.throt = PWMThrottle(PCA9685(throttle_id)) 53 | self.steer = PWMSteering(PCA9685(steering_id)) 54 | self.throt_limit = [-1.0, 1.0] 55 | self.steer_limit = [-1.0, 1.0] 56 | self.throt_direction = -1 57 | self.steer_direction = -1 58 | return 59 | 60 | def set_value(self, value, mode='steer'): 61 | if mode == 'throt': 62 | controller = self.throt 63 | limit = self.throt_limit 64 | direction = self.throt_direction 65 | elif mode == 'steer': 66 | controller = self.steer 67 | limit = self.steer_limit 68 | direction = self.steer_direction 69 | output_value = np.clip(value, limit[0], limit[1]) 70 | controller.run(output_value * direction) 71 | return output_value 72 | 73 | def shutdown(self): 74 | self.throt.shutdown() 75 | self.steer.shutdown() 76 | return 77 | 78 | 79 | def main(): 80 | ymlfile = open('config.yml') 81 | cfg = yaml.load(ymlfile) 82 | ymlfile.close() 83 | 84 | mp_receiver = MPMQTTReceiver(host=cfg['pi_ip']) 85 | controller = RCController() 86 | 87 | while True: 88 | throttle_value = mp_receiver.throttle.value 89 | steering_value = mp_receiver.steering.value 90 | 91 | time_diff_sec = abs(mp_receiver.time.value - time.time()) 92 | if time_diff_sec > 1.0: 93 | throttle_value = 0 94 | steering_value = 0 95 | 96 | throttle_output = controller.set_value(throttle_value, mode='throt') 97 | steering_output = controller.set_value(steering_value, mode='steer') 98 | 99 | print('Time diff: {}, Throttle: {}, Steering: {}'.format(time_diff_sec, throttle_output, steering_output)) 100 | time.sleep(0.01) 101 | 102 | controller.shutdown() 103 | mp_receiver.close() 104 | 105 | 106 | if __name__ == '__main__': 107 | main() 108 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | Adafruit-GPIO==1.0.3 2 | Adafruit-PCA9685==1.0.1 3 | Adafruit-PureIO==0.2.3 4 | paho-mqtt==1.4.0 5 | pynput==1.4.2 6 | pytest==4.6.0 7 | python-xlib==0.25 8 | xlib==0.21 9 | -------------------------------------------------------------------------------- /tests/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import platform 5 | 6 | 7 | def on_pi(): 8 | if 'arm' in platform.machine(): 9 | return True 10 | return False 11 | -------------------------------------------------------------------------------- /tests/test_actuator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import pytest 5 | from setup import on_pi 6 | 7 | import sys, os 8 | sys.path.append(os.path.join(os.path.dirname('__file__'), '../')) 9 | from parts.actuator import PCA9685, PWMSteering, PWMThrottle 10 | 11 | 12 | @pytest.mark.skipif(on_pi() == False, reason='Not on RPi') 13 | def test_PCA9685(): 14 | c = PCA9685(0) 15 | 16 | @pytest.mark.skipif(on_pi() == False, reason='Not on RPi') 17 | def test_PWMSteering(): 18 | c = PCA9685(0) 19 | s = PWMSteering(c) 20 | 21 | -------------------------------------------------------------------------------- /util/data.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Assorted functions for manipulating data. 5 | """ 6 | import numpy as np 7 | import itertools 8 | 9 | 10 | def linear_bin(a): 11 | """ 12 | Convert a value to a categorical array. 13 | 14 | Parameters 15 | ---------- 16 | a : int or float 17 | A value between -1 and 1 18 | 19 | Returns 20 | ------- 21 | list of int 22 | A list of length 15 with one item set to 1, which represents the linear value, and all other items set to 0. 23 | """ 24 | a = a + 1 25 | b = round(a / (2 / 14)) 26 | arr = np.zeros(15) 27 | arr[int(b)] = 1 28 | return arr 29 | 30 | 31 | def linear_unbin(arr): 32 | """ 33 | Convert a categorical array to value. 34 | 35 | See Also 36 | -------- 37 | linear_bin 38 | """ 39 | if not len(arr) == 15: 40 | raise ValueError('Illegal array length, must be 15') 41 | b = np.argmax(arr) 42 | a = b * (2 / 14) - 1 43 | return a 44 | 45 | 46 | def bin_Y(Y): 47 | """ 48 | Convert a list of values to a list of categorical arrays. 49 | 50 | Parameters 51 | ---------- 52 | Y : iterable of int 53 | Iterable with values between -1 and 1 54 | 55 | Returns 56 | ------- 57 | A two dimensional array of int 58 | 59 | See Also 60 | -------- 61 | linear_bin 62 | """ 63 | d = [ linear_bin(y) for y in Y ] 64 | return np.array(d) 65 | 66 | 67 | def unbin_Y(Y): 68 | """ 69 | Convert a list of categorical arrays to a list of values. 70 | 71 | See Also 72 | -------- 73 | linear_bin 74 | """ 75 | d = [ linear_unbin(y) for y in Y ] 76 | return np.array(d) 77 | 78 | 79 | def map_range(x, X_min, X_max, Y_min, Y_max): 80 | """ 81 | Linear mapping between two ranges of values 82 | """ 83 | X_range = X_max - X_min 84 | Y_range = Y_max - Y_min 85 | XY_ratio = X_range / Y_range 86 | 87 | y = ((x - X_min) / XY_ratio + Y_min) // 1 88 | 89 | return int(y) 90 | 91 | 92 | def merge_two_dicts(x, y): 93 | """ 94 | Given two dicts, merge them into a new dict as a shallow copy 95 | """ 96 | z = x.copy() 97 | z.update(y) 98 | return z 99 | 100 | 101 | def param_gen(params): 102 | """ 103 | Accepts a dictionary of parameter options and returns 104 | a list of dictionary with the permutations of the parameters. 105 | """ 106 | for p in itertools.product(*params.values()): 107 | yield dict(zip(params.keys(), p )) 108 | --------------------------------------------------------------------------------