2 # -*- coding: iso-8859-15 -*-
7 from i2c import i2c_write_reg, i2c_read_reg
23 speed1_wish=float(sys.argv[1])*STEP_PER_M_AVG
24 speed2_wish=float(sys.argv[1])*STEP_PER_M_AVG
25 speed3_wish=float(sys.argv[2])*STEP_PER_M_AVG
26 speed4_wish=float(sys.argv[2])*STEP_PER_M_AVG
28 def set_pwm(pwm1, pwm2, pwm3, pwm4):
29 i2c_write_reg(0x50, 0x1, struct.pack(">h", pwm1))
30 i2c_write_reg(0x50, 0x3, struct.pack(">h", pwm2))
31 i2c_write_reg(0x50, 0x5, struct.pack(">h", pwm3))
32 i2c_write_reg(0x50, 0x7, struct.pack(">h", pwm4))
34 if __name__ == "__main__":
36 speed1, speed2, speed3, speed4 = struct.unpack(">hhhh", i2c_read_reg(0x50, 0x30, 8))
37 error, = struct.unpack(">B", i2c_read_reg(0x50, 0xA1, 1))
44 e = speed1_wish - speed1
46 motor1 = KP*e + KI*PID_T*esum1 + KD/PID_T*(e - eold1)
49 if (motor1 < 0 and speed1_wish > 0): motor1 = PWM_BREAK
50 elif (motor1 > 0 and speed1_wish < 0): motor1 = PWM_BREAK
51 elif (motor1 > 255): motor1 = 255
52 elif (motor1 < -255): motor1 = -255
54 print "Wish=", speed1_wish, "Speed=", speed1, "e=", e, "esum=", esum1, "pwm=", motor1, "error=", error
61 e = speed2_wish - speed2
63 motor2 = KP*e + KI*PID_T*esum2 + KD/PID_T*(e - eold2)
66 if (motor2 < 0 and speed2_wish > 0): motor2 = PWM_BREAK
67 elif (motor2 > 0 and speed2_wish < 0): motor2 = PWM_BREAK
68 elif (motor2 > 255): motor2 = 255
69 elif (motor2 < -255): motor2 = -255
76 e = speed3_wish - speed3
78 motor3 = KP*e + KI*PID_T*esum3 + KD/PID_T*(e - eold3)
81 if (motor3 < 0 and speed3_wish > 0): motor3 = PWM_BREAK
82 elif (motor3 > 0 and speed3_wish < 0): motor3 = PWM_BREAK
83 elif (motor3 > 255): motor3 = 255
84 elif (motor3 < -255): motor3 = -255
91 e = speed4_wish - speed4
93 motor4 = KP*e + KI*PID_T*esum4 + KD/PID_T*(e - eold4)
96 if (motor4 < 0 and speed4_wish > 0): motor4 = PWM_BREAK
97 elif (motor4 > 0 and speed4_wish < 0): motor4 = PWM_BREAK
98 elif (motor4 > 255): motor4 = 255
99 elif (motor4 < -255): motor4 = -255
101 #print "Wish=", speed4_wish, "Speed=", speed4, "e=", e, "esum=", esum4, "pwm=", motor4, "error=", error
103 set_pwm(motor1, motor2, motor3, motor4)