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.06
  10 KI=0.10
  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 PWM_BREAK = -32768
  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
  27
  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))
  33
  34 if __name__ == "__main__":
  35         for i in range(200):
  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))
  38
  39                 if speed1_wish == 0:
  40                         motor1 = 0.0
  41                         eold1 = 0.0
  42                         esum1 = 0.0
  43                 else:
  44                         e = speed1_wish - speed1
  45                         esum1+=e
  46                         motor1 = KP*e + KI*PID_T*esum1 + KD/PID_T*(e - eold1)
  47                         eold1 = e
  48
  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
  53
  54                         print "Wish=", speed1_wish, "Speed=", speed1, "e=", e, "esum=", esum1, "pwm=", motor1, "error=", error
  55
  56                 if speed2_wish == 0:
  57                         motor2 = 0.0
  58                         eold2 = 0.0
  59                         esum2 = 0.0
  60                 else:
  61                         e = speed2_wish - speed2
  62                         esum2+=e
  63                         motor2 = KP*e + KI*PID_T*esum2 + KD/PID_T*(e - eold2)
  64                         eold2 = e
  65
  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
  70
  71                 if speed3_wish == 0:
  72                         motor3 = 0.0
  73                         eold3 = 0.0
  74                         esum3 = 0.0
  75                 else:
  76                         e = speed3_wish - speed3
  77                         esum3+=e
  78                         motor3 = KP*e + KI*PID_T*esum3 + KD/PID_T*(e - eold3)
  79                         eold3 = e
  80
  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
  85
  86                 if speed4_wish == 0:
  87                         motor4 = 0.0
  88                         eold4 = 0.0
  89                         esum4 = 0.0
  90                 else:
  91                         e = speed4_wish - speed4
  92                         esum4+=e
  93                         motor4 = KP*e + KI*PID_T*esum4 + KD/PID_T*(e - eold4)
  94                         eold4 = e
  95
  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
 100
 101                         #print "Wish=", speed4_wish, "Speed=", speed4, "e=", e, "esum=", esum4, "pwm=", motor4, "error=", error
 102
 103                 set_pwm(motor1, motor2, motor3, motor4)
 104
 105                 sleep(PID_T)
 106         set_pwm(0, 0, 0, 0)