├── images ├── esp32.PNG ├── proj.PNG ├── pygame-joystick.PNG ├── IMG_20200219_151055.jpg └── IMG_20200219_151108.jpg ├── esp32_src ├── main.py ├── bsp.py └── HLModbusSlave.py ├── joystick.py └── README.md /images/esp32.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/winxos/esp32_mecanum_wheel_joystick_wireless_car/HEAD/images/esp32.PNG -------------------------------------------------------------------------------- /images/proj.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/winxos/esp32_mecanum_wheel_joystick_wireless_car/HEAD/images/proj.PNG -------------------------------------------------------------------------------- /images/pygame-joystick.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/winxos/esp32_mecanum_wheel_joystick_wireless_car/HEAD/images/pygame-joystick.PNG -------------------------------------------------------------------------------- /images/IMG_20200219_151055.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/winxos/esp32_mecanum_wheel_joystick_wireless_car/HEAD/images/IMG_20200219_151055.jpg -------------------------------------------------------------------------------- /images/IMG_20200219_151108.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/winxos/esp32_mecanum_wheel_joystick_wireless_car/HEAD/images/IMG_20200219_151108.jpg -------------------------------------------------------------------------------- /esp32_src/main.py: -------------------------------------------------------------------------------- 1 | import socket 2 | def do_connect(): 3 | import network 4 | wlan = network.WLAN(network.STA_IF) 5 | wlan.active(True) 6 | if not wlan.isconnected(): 7 | print('connecting...') 8 | wlan.connect('HeLuo', '11111111') 9 | while not wlan.isconnected(): 10 | pass 11 | print('network config:', wlan.ifconfig()) 12 | do_connect() 13 | port = 10001 14 | s=socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 15 | s.bind(('0.0.0.0',port)) #绑定端口 16 | from bsp import car_handler 17 | print('listening.') 18 | while True: #接收数据 19 | data,addr=s.recvfrom(100) 20 | car_handler.receive_ascii(data) 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /esp32_src/bsp.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | from HLModbusSlave import ModbusRegister, ModbusSlave 3 | from machine import Pin, PWM 4 | class Motor: 5 | def __init__(self, in1, in2): 6 | self.in1 = PWM(Pin(in1)) 7 | self.in2 = PWM(Pin(in2)) 8 | 9 | def speed(self, v): 10 | if v>0: 11 | self.in1.duty(v) 12 | self.in2.duty(0) 13 | elif v<0: 14 | self.in1.duty(0) 15 | self.in2.duty(-v) 16 | else: 17 | self.in1.duty(0) 18 | self.in2.duty(0) 19 | 20 | 21 | motors = [Motor(23,22),Motor(21,19),Motor(18,5),Motor(17,16)] 22 | out_pin = [Pin(4, Pin.OUT),Pin(2, Pin.OUT)] 23 | 24 | is_move = False 25 | 26 | 27 | def car_move(a, b): 28 | global is_move 29 | is_move = True 30 | print(a) 31 | spds =[ ((a[i] >> 8) - (a[i] & 0x00ff)) / 255 for i in range(4)] 32 | for i in range(4): 33 | motors[i].speed(spds[i]*4) 34 | 35 | 36 | 37 | def car_io(a, b): 38 | if a[0] == 0xff00: 39 | out_pin[0].on() 40 | elif a[0] == 0x00: 41 | out_pin[0].off() 42 | 43 | 44 | 45 | regs = [ModbusRegister(0, 4, [0, 0, 0, 0], [], None, car_move), 46 | ModbusRegister(4, 1, [0], [], None, car_io), 47 | ] 48 | car_handler = ModbusSlave(1, regs, None) 49 | 50 | 51 | def monitor(): 52 | global is_move 53 | while True: 54 | if is_move: 55 | if time.perf_counter() - last_tick > 0.5: 56 | for i in range(4): 57 | motors[i].speed(0) 58 | car_handler.regs[0].reg = [0, 0, 0, 0] 59 | sleep(0.01) 60 | 61 | import _thread 62 | _thread.start_new_thread(monitor,()) 63 | def test(): 64 | print("test") 65 | for i in range(4): 66 | motors[i].speed(800) 67 | sleep(2) 68 | motors[i].speed(-800) 69 | sleep(2) 70 | motors[i].speed(0) 71 | if __name__ == "__main__": 72 | test() 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /esp32_src/HLModbusSlave.py: -------------------------------------------------------------------------------- 1 | """ 2 | MODBUS SLAVE 3 | winxos 20191220 4 | """ 5 | from binascii import unhexlify 6 | class ModbusRegister: 7 | def __init__(self, 8 | start, 9 | length, 10 | reg, 11 | dev, 12 | read= None, 13 | write= None): 14 | self.start, self.length = start, length 15 | self.reg, self.dev = reg, dev 16 | self.read, self.write = read, write 17 | 18 | 19 | class ModbusSlave: 20 | def __init__(self, address, regs, sender): 21 | self.regs = regs 22 | self.address = address 23 | self.send = sender 24 | 25 | def read_registers(self, addr, sz): 26 | """ 27 | 0x03 function 28 | :return: True/False, Value 29 | """ 30 | for reg in self.regs: 31 | if reg.start <= addr < reg.start + reg.length: 32 | if reg.read: 33 | reg.read(reg.reg, reg.dev) 34 | offset = addr - reg.start 35 | return True, reg.reg[offset: offset + sz] 36 | 37 | def write_register(self, addr, value): 38 | """ 39 | 0x06 function 40 | :return: True/False, Value 41 | """ 42 | for reg in self.regs: 43 | if reg.start <= addr < reg.start + reg.length: 44 | if reg.write: 45 | reg.reg[addr - reg.start] = value 46 | reg.write(reg.reg, reg.dev) 47 | return True, 0 48 | 49 | def write_registers(self, addr, sz, data): 50 | """ 51 | 0x10 function 52 | the [addr, addr+sz) should in the range of a ModbusRegister 53 | :return: True/False, Value 54 | """ 55 | for reg in self.regs: 56 | if reg.start <= addr and addr + sz < reg.start + reg.length: 57 | if reg.write: 58 | reg.reg[addr - reg.start:] = data[:] 59 | reg.write(reg.reg, reg.dev) 60 | return True, 0 61 | 62 | def deal(self, data: bytes): 63 | print(data) 64 | if data[0] != 0x00 and data[0] != self.address: 65 | return False 66 | if data[1] == 0x03: 67 | self.read_registers(data[2] * 256 + data[3], data[4] * 256 + data[5]) 68 | elif data[1] == 0x06: 69 | self.write_register(data[2] * 256 + data[3], data[4] * 256 + data[5]) 70 | elif data[1] == 0x10: 71 | self.write_registers(data[2] * 256 + data[3], data[4] * 256 + data[5], data[6:]) 72 | 73 | def receive(self, data): 74 | """ 75 | call this while received data 76 | :param data: bytes 77 | """ 78 | pass 79 | 80 | def receive_ascii(self, data): 81 | """ 82 | modbus ascii protocol 83 | :return: True/False, Value 84 | """ 85 | if not data.startswith(":") or not data.endswith("\r\n") or len(data) % 2 == 0: 86 | return False 87 | mdata = unhexlify(data[1:-2]) 88 | print(mdata) 89 | if sum(mdata) % 0x100 != 0: 90 | return False 91 | self.deal(mdata[:-1]) 92 | 93 | 94 | def rd(a, b): 95 | print(a) 96 | 97 | 98 | def wt(a, b): 99 | print(a) 100 | 101 | 102 | def s(n): 103 | print(n) 104 | 105 | 106 | r = [1, 2, 3] 107 | d = [4, 5, 6] 108 | 109 | 110 | def test(): 111 | regs = [ModbusRegister(0, 4, r, d, rd, wt)] 112 | a = ModbusSlave(1, regs, s) 113 | a.read_registers(1, 1) 114 | a.write_registers(1, 2, [5, 6]) 115 | a.receive_ascii(":010300000001FB\r\n") 116 | 117 | 118 | if __name__ == '__main__': 119 | test() 120 | 121 | 122 | -------------------------------------------------------------------------------- /joystick.py: -------------------------------------------------------------------------------- 1 | """ 2 | Mecanum wheel control by joystick 3 | Using MODBUS-ASCII protocol 4 | python 3.8 5 | winxos 20191231 6 | """ 7 | import pygame 8 | import socket 9 | import binascii 10 | 11 | 12 | ADDR = ("192.168.0.106", 10001) # your esp32 ip address 13 | s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # udp socket 14 | 15 | 16 | def create_cmd(reg, x1): 17 | """ 18 | Modbus Ascii protocol frame builder 19 | :param reg: Modbus Register address 20 | :param x1: value 21 | :return: Modbus Ascii frame 22 | """ 23 | c = bytearray([1, 6, 0, reg, 0, 0]) 24 | # Modbus value have 2 bytes, i use the first byte as positive and the second byte as negative 25 | if x1 > 0: 26 | c[4] = x1 27 | else: 28 | c[5] = -x1 29 | x = sum(c) % 256 30 | # x is LRC check 31 | if x == 0: 32 | c.append(0) 33 | else: 34 | c.append(256 - x) 35 | return f":{binascii.b2a_hex(c).decode()}\r\n" 36 | 37 | 38 | # This is a simple class that will help us print to the screen 39 | # It has nothing to do with the joysticks, just outputting the 40 | # information. 41 | BLACK = (0, 0, 0) 42 | WHITE = (255, 255, 255) 43 | 44 | 45 | class TextPrint: 46 | def __init__(self): 47 | self.reset() 48 | self.font = pygame.font.Font(None, 20) 49 | 50 | def print(self, screen, textString): 51 | textBitmap = self.font.render(textString, True, BLACK) 52 | screen.blit(textBitmap, [self.x, self.y]) 53 | self.y += self.line_height 54 | 55 | def reset(self): 56 | self.x = 10 57 | self.y = 10 58 | self.line_height = 15 59 | 60 | def indent(self): 61 | self.x += 10 62 | 63 | def unindent(self): 64 | self.x -= 10 65 | 66 | 67 | pygame.init() 68 | 69 | # Set the width and height of the screen [width,height] 70 | size = [400, 200] 71 | screen = pygame.display.set_mode(size) 72 | 73 | pygame.display.set_caption("My Game") 74 | 75 | # Loop until the user clicks the close button. 76 | done = False 77 | 78 | # Used to manage how fast the screen updates 79 | clock = pygame.time.Clock() 80 | 81 | # Initialize the joysticks 82 | pygame.joystick.init() 83 | 84 | # Get ready to print 85 | textPrint = TextPrint() 86 | 87 | # -------- Main Program Loop ----------- 88 | while done == False: 89 | for event in pygame.event.get(): # User did something 90 | if event.type == pygame.QUIT: # If user clicked close 91 | done = True # Flag that we are done so we exit this loop 92 | 93 | # Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION 94 | if event.type == pygame.JOYBUTTONDOWN: 95 | print("Joystick button pressed.") 96 | if event.type == pygame.JOYBUTTONUP: 97 | print("Joystick button released.") 98 | screen.fill(WHITE) 99 | textPrint.reset() 100 | 101 | # Get count of joysticks 102 | joystick_count = pygame.joystick.get_count() 103 | if joystick_count > 0: 104 | joystick = pygame.joystick.Joystick(0) 105 | joystick.init() 106 | name = joystick.get_name() 107 | textPrint.print(screen, "Joystick name: {}".format(name)) 108 | axes = joystick.get_numaxes() 109 | textPrint.print(screen, "Number of axes: {}".format(axes)) 110 | textPrint.indent() 111 | for i in range(axes): 112 | axis = joystick.get_axis(i) 113 | textPrint.print(screen, "Axis {} value: {:>6.3f}".format(i, axis)) 114 | textPrint.unindent() 115 | 116 | vx = joystick.get_axis(0) 117 | vy = joystick.get_axis(1) 118 | vz = joystick.get_axis(4) 119 | if abs(vx) > 0.2 or abs(vy) > 0.2 or abs(vz) > 0.2: 120 | if abs(vx) < 0.2: 121 | vx = 0 122 | if abs(vy) < 0.2: 123 | vy = 0 124 | # spd is your 4 motor's speed, the +- sign depends on your wheel install type and motor direct, 125 | # you may need to read some article about Mecanum wheel to handler it. 126 | spd = [vx + vy + vz, vx - vy - vz, vx + vy - vz, vx - vy + vz] 127 | print(f"{vx:.2f} {vy:.2f} {vz:.2f}") 128 | for j in range(4): 129 | t = int(spd[j] * 250) 130 | if t > 255: 131 | t = 255 132 | if t < -255: 133 | t = -255 134 | # constrict the speed to range [-255,255] 135 | s.sendto(create_cmd(j, t).encode(), ADDR) 136 | pygame.display.flip() 137 | clock.tick(20) 138 | 139 | # Close the window and quit. 140 | # If you forget this line, the program will 'hang' 141 | # on exit if running from IDLE. 142 | pygame.quit() 143 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # esp32_Mecanum_wheels_joystick_wireless_car 2 | > winxos 20200219 3 | 4 | [toc] 5 | 6 | ### Brief Description 7 | 8 | It's a self made wireless Mecanum wheels car controlled by joystick. 9 | 10 | Just for fun. 11 | 12 | ![proj]( ./images/proj.PNG ) 13 | 14 | You can found operate video here 15 | 16 | https://www.youtube.com/watch?v=rDZgJA3lPdM 17 | 18 | https://www.youtube.com/watch?v=goBTG-G6iwU 19 | 20 | ### Technical Details 21 | 22 | #### System principle 23 | 24 | 1. Get the input of the joystick 25 | 2. According to the input signal, calculate the car motion parameters, generate the control commands. 26 | 3. Send the commands to the car. 27 | 4. The car received the commands, then analyze the commands, execute the commands to control motors. 28 | 5. repeat step 1. 29 | 30 | #### Joystick code 31 | 32 | File joystrick.py implement a simplest joystick controller in python, using pygame library. It runs like 33 | 34 | ![pygame-joystick]( ./images/pygame-joystick.PNG ) 35 | 36 | I used modbus-ascii as the communication protocol, if you are a newbie at modbus, you may need to look http://www.simplymodbus.ca/index.html first. 37 | 38 | The advantage of using a standard protocol is that you can reuse this part of the code. 39 | 40 | ```python 41 | def create_cmd(reg, x1): 42 | """ 43 | Modbus Ascii protocol frame builder 44 | :param reg: Modbus Register address 45 | :param x1: value 46 | :return: Modbus Ascii frame 47 | """ 48 | c = bytearray([1, 6, 0, reg, 0, 0]) 49 | # Modbus value have 2 bytes, i use the first byte as positive and the second byte as negative 50 | if x1 > 0: 51 | c[4] = x1 52 | else: 53 | c[5] = -x1 54 | x = sum(c) % 256 55 | # x is LRC check 56 | if x == 0: 57 | c.append(0) 58 | else: 59 | c.append(256 - x) 60 | return f":{binascii.b2a_hex(c).decode()}\r\n" 61 | ``` 62 | 63 | function `create_cmd` can build modbus ascii frame to send. 64 | 65 | yes , modbus ascii protocol is very simple and easy to use. 66 | 67 | ```python 68 | vx = joystick.get_axis(0) 69 | vy = joystick.get_axis(1) 70 | vz = joystick.get_axis(4) 71 | if abs(vx) > 0.2 or abs(vy) > 0.2 or abs(vz) > 0.2: 72 | if abs(vx) < 0.2: 73 | vx = 0 74 | if abs(vy) < 0.2: 75 | vy = 0 76 | # spd is your 4 motor's speed, the +- sign depends on your wheel install type and motor direct, 77 | # you may need to read some article about Mecanum wheel to handler it. 78 | spd = [vx + vy + vz, vx - vy - vz, vx + vy - vz, vx - vy + vz] 79 | print(f"{vx:.2f} {vy:.2f} {vz:.2f}") 80 | for j in range(4): 81 | t = int(spd[j] * 250) 82 | if t > 255: 83 | t = 255 84 | if t < -255: 85 | t = -255 86 | # constrict the speed to range [-255,255] 87 | s.sendto(create_cmd(j, t).encode(), ADDR) 88 | ``` 89 | 90 | This part of the code contains the kinematics calculation of Mecanum wheels and instruction sending. 91 | 92 | You need to set your pc ip address at the first 93 | 94 | ```ADDR = ("192.168.0.106", 10001) # your esp32 ip address``` 95 | 96 | That's joystick.py all. 97 | 98 | #### Car Body 99 | 100 | It's the cheapest Mecanum wheels car I can design. 101 | 102 | ![IMG_20200219_151055]( ./images/IMG_20200219_151055.jpg ) 103 | 104 | ![IMG_20200219_151108]( ./images/IMG_20200219_151108.jpg ) 105 | 106 | BOM 107 | 108 | | Name | Price\$ each | Price\$ total | 109 | | ------- | ---------- | ---------- | 110 | | wheel | 3 |12| 111 | | esp32 | 4 |4| 112 | | 18650 batteries | 1 |2| 113 | | drivers and others | 3 |3| 114 | | **total** | |21| 115 | 116 | #### Car code 117 | 118 | The car code in esp32_src directory. 119 | 120 | I use micropython to develop, 121 | 122 | so first you need to burn micropython firmware to your esp32, then you can use some software to develop such as uPyCraft. 123 | 124 | ![esp32]( ./images/esp32.PNG ) 125 | 126 | ##### main.py 127 | 128 | When you upload the three files to the esp32, the main.py will start up automatically , 129 | 130 | you need to change the code in main.py 131 | 132 | ```python 133 | wlan.connect('HeLuo', '11111111') 134 | ``` 135 | 136 | to you WIFI SSID and password. 137 | 138 | ##### bsp.py 139 | 140 | this file determine the io port connection, 141 | 142 | you need to change the code 143 | 144 | ```python 145 | motors = [Motor(23,22),Motor(21,19),Motor(18,5),Motor(17,16)] 146 | ``` 147 | 148 | to the right sequence, the same as your motors connection. 149 | 150 | ##### HLModbusSlave.py 151 | 152 | this file is a simplest modbus slave module, very rough implementation. 153 | 154 | ### The End 155 | 156 | Make a wireless controll Mecanum wheels car is a big challenge to a newbie, 157 | 158 | You need to know thing about mechanics, mathematics, circuit, esp32, python, socket communication etc， 159 | 160 | But when you finished this project, you will feel very satisfied and more motivated to challenge more difficult projects. 161 | 162 | the project address is https://github.com/winxos/esp32_mecanum_wheel_joystick_wireless_car 163 | 164 | Writing a good popular technical articles is not easy, I will post more interesting items in my free time, wish you like. 165 | 166 | Click star, Follow me, is the greatest encouragement to the author. 167 | 168 | 169 | 170 | 171 | 172 | --------------------------------------------------------------------------------