pid tuning for gear ratio=1:47
[ros_wild_thumper.git] / scripts / pid_manual.py
1 #!/usr/bin/env python
2 # -*- coding: iso-8859-15 -*-
3
4 import sys
5 import struct
6 from time import sleep
7 from i2c import i2c_write_reg, i2c_read_reg
8
9 KP=0.05
10 KI=0.0
11 KD=0.0
12 PID_T=0.01
13 STEP_PER_M_AVG=5766.1
14 eold1 = 0.0
15 eold2 = 0.0
16 eold3 = 0.0
17 eold4 = 0.0
18 esum1 = 0.0
19 esum2 = 0.0
20 esum3 = 0.0
21 esum4 = 0.0
22 speed1_wish=float(sys.argv[1])*STEP_PER_M_AVG
23 speed2_wish=float(sys.argv[1])*STEP_PER_M_AVG
24 speed3_wish=float(sys.argv[2])*STEP_PER_M_AVG
25 speed4_wish=float(sys.argv[2])*STEP_PER_M_AVG
26
27 def set_pwm(pwm1, pwm2, pwm3, pwm4):
28         i2c_write_reg(0x50, 0x1, struct.pack(">h", pwm1))
29         i2c_write_reg(0x50, 0x3, struct.pack(">h", pwm2))
30         i2c_write_reg(0x50, 0x5, struct.pack(">h", pwm3))
31         i2c_write_reg(0x50, 0x7, struct.pack(">h", pwm4))
32
33 if __name__ == "__main__":
34         for i in range(200):
35                 speed1, speed2, speed3, speed4 = struct.unpack(">hhhh", i2c_read_reg(0x50, 0x30, 8))
36                 error,  = struct.unpack(">B", i2c_read_reg(0x50, 0xA1, 1))
37
38                 if speed1_wish == 0: 
39                         motor1 = 0.0
40                         eold1 = 0.0
41                         esum1 = 0.0
42                 else:
43                         e = speed1_wish - speed1
44                         esum1+=e
45                         motor1 = KP*e + KI*PID_T*esum1 + KD/PID_T*(e - eold1)
46                         eold1 = e
47
48                         if (motor1 < 0 and speed1_wish > 0): motor1 = 0 
49                         elif (motor1 > 0 and speed1_wish < 0): motor1 = 0       
50                         elif (motor1 > 255): motor1 = 255
51                         elif (motor1 < -255): motor1 = -255
52
53                 if speed2_wish == 0: 
54                         motor2 = 0.0
55                         eold2 = 0.0
56                         esum2 = 0.0
57                 else:
58                         e = speed2_wish - speed2
59                         esum2+=e
60                         motor2 = KP*e + KI*PID_T*esum2 + KD/PID_T*(e - eold2)
61                         eold2 = e
62
63                         if (motor2 < 0 and speed2_wish > 0): motor2 = 0 
64                         elif (motor2 > 0 and speed2_wish < 0): motor2 = 0       
65                         elif (motor2 > 255): motor2 = 255
66                         elif (motor2 < -255): motor2 = -255
67
68                 if speed3_wish == 0: 
69                         motor3 = 0.0
70                         eold3 = 0.0
71                         esum3 = 0.0
72                 else:
73                         e = speed3_wish - speed3
74                         esum3+=e
75                         motor3 = KP*e + KI*PID_T*esum3 + KD/PID_T*(e - eold3)
76                         eold3 = e
77
78                         if (motor3 < 0 and speed3_wish > 0): motor3 = 0 
79                         elif (motor3 > 0 and speed3_wish < 0): motor3 = 0       
80                         elif (motor3 > 255): motor3 = 255
81                         elif (motor3 < -255): motor3 = -255
82
83                 if speed4_wish == 0: 
84                         motor4 = 0.0
85                         eold4 = 0.0
86                         esum4 = 0.0
87                 else:
88                         e = speed4_wish - speed4
89                         esum4+=e
90                         motor4 = KP*e + KI*PID_T*esum4 + KD/PID_T*(e - eold4)
91                         eold4 = e
92
93                         if (motor4 < 0 and speed4_wish > 0): motor4 = 0 
94                         elif (motor4 > 0 and speed4_wish < 0): motor4 = 0       
95                         elif (motor4 > 255): motor4 = 255
96                         elif (motor4 < -255): motor4 = -255
97
98                         print "Wish=", speed4_wish, "Speed=", speed4, "e=", e, "esum=", esum4, "pwm=", motor4, "error=", error
99
100                 set_pwm(motor1, motor2, motor3, motor4)
101
102                 sleep(PID_T)
103         set_pwm(0, 0, 0, 0)