commit by robo pro coding

RoboMission_linepid.py

@@ -27,6 +27,9 @@ line_last = None
 ts_diff0 = None
 ts_diff = None
 Kd = None
+p = None
+i = None
+d = None
 ts0 = None
 sampletime = None
 ts1 = None
@@ -34,17 +37,17 @@ ts_offset = None
 
 
 def initPID():
-    global speed, i1, speed_slow, i2, speed_fwd, calib_i1_min, calib_i1_max, white, mode, speed_m1, calib_i2_min, calib_i2_max, line, control, speed_m2, Kp, Ki, line_last, ts_diff0, ts_diff, Kd, ts0, sampletime, ts1, ts_offset
+    global speed, i1, speed_slow, i2, speed_fwd, calib_i1_min, calib_i1_max, white, mode, speed_m1, calib_i2_min, calib_i2_max, line, control, speed_m2, Kp, Ki, line_last, ts_diff0, ts_diff, Kd, p, i, d, ts0, sampletime, ts1, ts_offset
     global pid
     from simple_pid import PID
-    pid = PID(0.01, 0.0001, 0.001)
+    pid = PID(0.007, 0.09, 0.0005)
     pid.sample_time = sampletime
     pid.setpoint = 0.0
     pid.output_limits = (-512, 512)
 
 
 def readSensors():
-    global speed, i1, speed_slow, i2, speed_fwd, calib_i1_min, calib_i1_max, white, mode, speed_m1, calib_i2_min, calib_i2_max, line, control, speed_m2, Kp, Ki, line_last, ts_diff0, ts_diff, Kd, ts0, sampletime, ts1, ts_offset
+    global speed, i1, speed_slow, i2, speed_fwd, calib_i1_min, calib_i1_max, white, mode, speed_m1, calib_i2_min, calib_i2_max, line, control, speed_m2, Kp, Ki, line_last, ts_diff0, ts_diff, Kd, p, i, d, ts0, sampletime, ts1, ts_offset
     i1 = ft_math.map(TXT_M_I1_color_sensor.get_voltage(), calib_i1_min, calib_i1_max, 100, 0)
     i2 = ft_math.map(TXT_M_I2_color_sensor.get_voltage(), calib_i2_min, calib_i2_max, 100, 0)
     white = i1 > 90 and i2 > 90
@@ -52,16 +55,16 @@ def readSensors():
 
 
 def calcSpeed():
-    global speed, i1, speed_slow, i2, speed_fwd, calib_i1_min, calib_i1_max, white, mode, speed_m1, calib_i2_min, calib_i2_max, line, control, speed_m2, Kp, Ki, line_last, ts_diff0, ts_diff, Kd, ts0, sampletime, ts1, ts_offset
+    global speed, i1, speed_slow, i2, speed_fwd, calib_i1_min, calib_i1_max, white, mode, speed_m1, calib_i2_min, calib_i2_max, line, control, speed_m2, Kp, Ki, line_last, ts_diff0, ts_diff, Kd, p, i, d, ts0, sampletime, ts1, ts_offset
     speed = round(control) * 2
     if speed > 0:
         mode = 1
-        speed_m1 = min(max(speed_fwd - speed, -512), -speed_slow)
-        speed_m2 = min(max(speed_fwd + speed, speed_slow), 512)
+        speed_m1 = min(max(speed_fwd - speed, -512), 0)
+        speed_m2 = min(max(speed_fwd + speed, 0), 512)
     elif speed < 0:
         mode = -1
-        speed_m2 = min(max(speed_fwd - speed, -512), -speed_slow)
-        speed_m1 = min(max(speed_fwd + speed, speed_slow), 512)
+        speed_m2 = min(max(speed_fwd - speed, -512), 0)
+        speed_m1 = min(max(speed_fwd + speed, 0), 512)
     else:
         mode = 0
         speed_m1 = speed_fwd
@@ -69,7 +72,7 @@ def calcSpeed():
 
 
 def initDisplay():
-    global speed, i1, speed_slow, i2, speed_fwd, calib_i1_min, calib_i1_max, white, mode, speed_m1, calib_i2_min, calib_i2_max, line, control, speed_m2, Kp, Ki, line_last, ts_diff0, ts_diff, Kd, ts0, sampletime, ts1, ts_offset
+    global speed, i1, speed_slow, i2, speed_fwd, calib_i1_min, calib_i1_max, white, mode, speed_m1, calib_i2_min, calib_i2_max, line, control, speed_m2, Kp, Ki, line_last, ts_diff0, ts_diff, Kd, p, i, d, ts0, sampletime, ts1, ts_offset
     Kp = pid.Kp
     Ki = pid.Ki
     Kd = pid.Kd
@@ -79,13 +82,14 @@ def initDisplay():
 
 
 def threadDisplay():
-    global speed, i1, speed_slow, i2, speed_fwd, calib_i1_min, calib_i1_max, white, mode, speed_m1, calib_i2_min, calib_i2_max, line, control, speed_m2, Kp, Ki, line_last, ts_diff0, ts_diff, Kd, ts0, sampletime, ts1, ts_offset
+    global speed, i1, speed_slow, i2, speed_fwd, calib_i1_min, calib_i1_max, white, mode, speed_m1, calib_i2_min, calib_i2_max, line, control, speed_m2, Kp, Ki, line_last, ts_diff0, ts_diff, Kd, p, i, d, ts0, sampletime, ts1, ts_offset
     while True:
         if white:
             print('ts:{:.2f} ({:.2f}) white:{} i12:{} {}'.format(ts_diff * 1000, ts_diff0 * 1000, white, i1, i2))
         else:
             if line != line_last:
-                print('ts:{:.2f} ({:.2f}) line:{} mode:{} control:{:.3f} speed:{} speed_fwd:{} i12:{} {} m12: {} {}'.format(ts_diff * 1000, ts_diff0 * 1000, line, mode, control, speed, speed_fwd, i1, i2, speed_m1, speed_m2))
+                p, i, d = pid.components
+                print('ts:{:.2f} ({:.2f}) line:{} mode:{} control:{:.3f} speed:{} speed_fwd:{} i12:{} {} m12: {} {} pid: {:.2f} {:.2f} {:.2f}'.format(ts_diff * 1000, ts_diff0 * 1000, line, mode, control, speed, speed_fwd, i1, i2, speed_m1, speed_m2, p, i, d))
                 line_last = line
         if ts_diff0 > ts_diff:
             pass
@@ -93,8 +97,8 @@ def threadDisplay():
 
 
 speed = 512
-speed_slow = 150
-speed_fwd = 250
+speed_slow = 110
+speed_fwd = 512
 speed_m1 = 0
 speed_m2 = 0
 calib_i1_min = 180
@@ -119,10 +123,12 @@ while True:
     readSensors()
     control = pid(line)
     if white:
+        pid.automode = False
         speed = 0
         TXT_M_M1_encodermotor.stop()
         TXT_M_M2_encodermotor.stop()
     else:
+        pid.set_auto_mode(True, last_output=0.0)
         calcSpeed()
         TXT_M_M1_encodermotor.set_speed(int(speed_m1), Motor.CCW)
         TXT_M_M1_encodermotor.start()