初始化提交

MecanumVehicle.py

@@ -0,0 +1,171 @@
+#!/usr/bin/env python3
+
+import time
+import struct
+import socket
+import RPi.GPIO as GPIO
+
+def clamp(n, a, b): 
+
+    if n < a:
+    
+        return a
+        
+    elif n > b:
+    
+        return b
+
+    else: 
+    
+        return n 
+
+interval = 0.001
+
+udp_addr = ("192.168.1.1", 6000)
+
+GPIO.setmode(GPIO.BCM)
+
+motor_pins = [4, 18, 17, 27, 23, 22, 24, 25]
+motor_pwms = []
+
+def init_motor():
+
+    GPIO.setup(motor_pins, GPIO.OUT)
+    
+    for motor_index in range(0, 8):
+    
+        motor_pwms.append(GPIO.PWM(motor_pins[motor_index], 500))
+        motor_pwms[motor_index].start(0)
+
+motor_LF_forward = 0
+motor_LF_reverse = 1
+
+motor_RF_forward = 3
+motor_RF_reverse = 2
+    
+motor_LR_forward = 5
+motor_LR_reverse = 4
+
+motor_RR_forward = 6
+motor_RR_reverse = 7
+
+def stop_motor():
+
+    for motor_index in range(0, 8):
+    
+        motor_pwms[motor_index].ChangeDutyCycle(0.0)
+
+def set_motor(motor, speed, is_brake = False):
+
+    motor_forward = -1
+    motor_reverse = -1
+
+    if motor == "LF":
+    
+        motor_forward = motor_LF_forward
+        motor_reverse = motor_LF_reverse
+    
+    elif motor == "RF":
+    
+        motor_forward = motor_RF_forward
+        motor_reverse = motor_RF_reverse
+    
+    elif motor == "LR":
+    
+        motor_forward = motor_LR_forward
+        motor_reverse = motor_LR_reverse
+    
+    else: # motor == "RR"
+    
+        motor_forward = motor_RR_forward
+        motor_reverse = motor_RR_reverse
+    
+    if is_brake == True:
+        
+        motor_pwms[motor_forward].ChangeDutyCycle(100.0)
+        motor_pwms[motor_reverse].ChangeDutyCycle(100.0)
+    
+    elif speed < 0:
+
+        motor_pwms[motor_forward].ChangeDutyCycle(0.0)
+        motor_pwms[motor_reverse].ChangeDutyCycle(-speed * 100.0)
+
+    elif speed > 0:
+    
+        motor_pwms[motor_forward].ChangeDutyCycle(speed * 100.0)
+        motor_pwms[motor_reverse].ChangeDutyCycle(0.0)
+
+    else:
+
+        motor_pwms[motor_forward].ChangeDutyCycle(0.0)
+        motor_pwms[motor_reverse].ChangeDutyCycle(0.0)
+
+sock = socket.socket()
+lasttime = 0
+
+def init_udp():
+
+    global sock
+    
+    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    sock.bind(("0.0.0.0", 6000))
+    sock.setblocking(False)
+
+def hand_udp():
+
+    global sock
+    global udp_addr
+    global lasttime
+
+    try:
+
+        (data, source) = sock.recvfrom(1024)
+        
+        udp_addr = source
+        
+        if len(data) == 6:
+
+            seq, speed_LF, speed_RF, speed_LR, speed_RR, is_brake = struct.unpack("!Bbbbb?", data)
+
+            set_motor("LF", clamp(speed_LF / 128, -1.0, 1.0), is_brake)
+            set_motor("RF", clamp(speed_RF / 128, -1.0, 1.0), is_brake)
+            set_motor("LR", clamp(speed_LR / 128, -1.0, 1.0), is_brake)
+            set_motor("RR", clamp(speed_RR / 128, -1.0, 1.0), is_brake)
+
+            sock.sendto(struct.pack("!B", seq), udp_addr)
+            
+            lasttime = time.time()
+
+    except socket.error:
+    
+        pass 
+
+    if time.time() - lasttime >= 0.5:
+    
+        set_motor("LF", 0.0, True)
+        set_motor("RF", 0.0, True)
+        set_motor("LR", 0.0, True)
+        set_motor("RR", 0.0, True)
+
+try:
+
+    init_motor()
+    init_udp()
+
+    while True:
+
+        hand_udp()
+
+        time.sleep(interval)
+
+except KeyboardInterrupt:
+
+    print("\nKeyboardInterrupt")
+
+finally:
+
+    stop_motor()
+
+    GPIO.cleanup()
+
+    print("Exit")