pid tuning for gear ratio=1:47

[ros_wild_thumper.git] / scripts / pid_manual.py

diff --git a/scripts/pid_manual.py b/scripts/pid_manual.py
index 77a7bce3f39a40cbef907d60296db2f6f5cfb313..9b6b60a1781eab92ee1a6119b641471abf4ead21 100755 (executable)
--- a/scripts/pid_manual.py
+++ b/scripts/pid_manual.py
@@ -6,11 +6,11 @@ import struct
 from time import sleep
 from i2c import i2c_write_reg, i2c_read_reg
 
 from time import sleep
 from i2c import i2c_write_reg, i2c_read_reg
 

-KP=0.034
-KI=0.02
+KP=0.05
+KI=0.0

 KD=0.0
 PID_T=0.01
 KD=0.0
 PID_T=0.01

-STEP_PER_M_AVG=4171.4
+STEP_PER_M_AVG=5766.1

 eold1 = 0.0
 eold2 = 0.0
 eold3 = 0.0
 eold1 = 0.0
 eold2 = 0.0
 eold3 = 0.0


@@ -80,8 +80,6 @@ if __name__ == "__main__":
                        elif (motor3 > 255): motor3 = 255
                        elif (motor3 < -255): motor3 = -255
 
                        elif (motor3 > 255): motor3 = 255
                        elif (motor3 < -255): motor3 = -255
 

-                       print "Wish=", speed3_wish, "Speed=", speed3, "e=", e, "esum=", esum3, "pwm=", motor3, "error=", error
-

                if speed4_wish == 0: 
                        motor4 = 0.0
                        eold4 = 0.0
                if speed4_wish == 0: 
                        motor4 = 0.0
                        eold4 = 0.0


@@ -97,6 +95,8 @@ if __name__ == "__main__":
                        elif (motor4 > 255): motor4 = 255
                        elif (motor4 < -255): motor4 = -255
 
                        elif (motor4 > 255): motor4 = 255
                        elif (motor4 < -255): motor4 = -255
 

+                       print "Wish=", speed4_wish, "Speed=", speed4, "e=", e, "esum=", esum4, "pwm=", motor4, "error=", error
+

                set_pwm(motor1, motor2, motor3, motor4)
 
                sleep(PID_T)
                set_pwm(motor1, motor2, motor3, motor4)
 
                sleep(PID_T)