Revision 1:150bafe1bd61, committed 2017-09-07

Comitter:

yxyang

Date:

Thu Sep 07 20:15:51 2017 +0000

Parent:

0:4856445a8db9

Commit message:

Partial checkpoint: autostop working, needs better integration with camera

Changed in this revision

diff -r 4856445a8db9 -r 150bafe1bd61 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Thu Sep 07 20:15:51 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

diff -r 4856445a8db9 -r 150bafe1bd61 RPCInterface.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RPCInterface.lib	Thu Sep 07 20:15:51 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/abuchan/code/RPCInterface/#1b85f3d0fae9

diff -r 4856445a8db9 -r 150bafe1bd61 main.cpp
--- a/main.cpp	Tue May 30 06:43:31 2017 +0000
+++ b/main.cpp	Thu Sep 07 20:15:51 2017 +0000
@@ -17,25 +17,31 @@
 /***** Constants ******/
 
 MPU6050 mpu6050;
+Serial pc(USBTX, USBRX);
 
-#define CAM_INTEGRATION_TIME 80
+#define CAM_INTEGRATION_TIME 200
+
+#define RAISE_ACCELERATION 0.97
+#define RAISE_THRESHOLD 10
+#define FALL_ACCELERATION 1.4
+#define FALL_THRESHOLD 3
 
 // Higher line threshold -> the sensor will only recognize larger changes in
 // brightness as a line edge
-#define LINE_THRESHOLD 3000
+#define LINE_THRESHOLD 1000
 #define LINE_PRECISION 2
 #define LINE_CROP_AMOUNT 4
 
 // These constants define the base pwm across the motors and how much the
 // controller
 // adjusts based on position of the line relative to the sensor
-#define SPEED_PWM 0.23
+#define SPEED_PWM 0.2
 #define TURN_SENS_INNER 1.5F
 #define TURN_SENS_OUTER 0.5F
 
 // Defines data
 #define LINE_HIST_SIZE 1000
-#define LINE_END_TIME 2500
+#define LINE_END_TIME 250
 
 // Sensor pins
 #define clk p16
@@ -43,380 +49,307 @@
 #define adc p18
 
 // Motors -- As labelled on the Zumy mbed board
-PwmOut m1_fwd (p21);
-PwmOut m1_back (p22);
+PwmOut m1_fwd(p21);
+PwmOut m1_back(p22);
 
-PwmOut m2_fwd (p23);
-PwmOut m2_back (p24);
+PwmOut m2_fwd(p23);
+PwmOut m2_back(p24);
 
 // LEDs
-DigitalOut led1 (LED1);
-DigitalOut led2 (LED2);
-DigitalOut led3 (LED3);
-DigitalOut led4 (LED4);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
 
-// USB serial
-Serial pc (USBTX, USBRX);
+float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, vel_z, pos_z;
+int r_enc, l_enc;
 
 // Data storage
 uint8_t line_hist[LINE_HIST_SIZE];
-int16_t accel_x[LINE_HIST_SIZE];
-int16_t accel_y[LINE_HIST_SIZE];
-int16_t accel_z[LINE_HIST_SIZE];
-int16_t gyro_x[LINE_HIST_SIZE];
-int16_t gyro_y[LINE_HIST_SIZE];
-int16_t gyro_z[LINE_HIST_SIZE];
 
 uint16_t hist_index;
 
 /***** Helper functions *****/
-float max (float a, float b);
+float max(float a, float b);
 
 // Sets the 4 LEDS on the mbed board to the four given binary values
-void setLEDs (uint8_t a, uint8_t b, uint8_t c, uint8_t d);
+void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d);
 
 // Steers by linearly decreasing the inner wheel speed as the line moves from
 // the center (unused in this program)
-void steerStupid (int8_t line_pos);
+void steerStupid(int8_t line_pos);
 
 // Based on steerStupid; adds the ability for the inner wheel to rotate in
 // reverse (unused in this program)
-void steerPointTurns (int8_t line_pos);
+void steerPointTurns(int8_t line_pos);
 
 // Based on steerPointTurns; the outer wheel moves faster the farther the line
 // is from center
-void steerImprovedPointTurns (int8_t line_pos);
+void steerImprovedPointTurns(int8_t line_pos);
 
-int
-main ()
-{
+int main() {
   /********** SENSOR SETUP **********/
-  TSL1401CL cam (clk, si, adc);
-  cam.setIntegrationTime (CAM_INTEGRATION_TIME);
+  TSL1401CL cam(clk, si, adc);
+  cam.setIntegrationTime(CAM_INTEGRATION_TIME);
 
   int8_t line_pos = -1;
   int8_t line_pos_previous = -1;
+  uint8_t raise_time = 0;
   uint8_t line_lost_time = 0;
 
   hist_index = 0;
+  vel_z = 0;
+  pos_z = 0;
 
-  i2c.frequency (400000);
+  i2c.frequency(400000);
 
   volatile bool imu_ready = false;
 
-  wait_ms (100);
-  uint8_t whoami = mpu6050.readByte (MPU6050_ADDRESS, WHO_AM_I_MPU6050);
-
-  if (whoami == 0x68) // WHO_AM_I should always be 0x68
-    {
-      mpu6050.MPU6050SelfTest (SelfTest);
-      if (SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f
-          && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f)
-        {
-          mpu6050.resetMPU6050 ();
-          mpu6050.calibrateMPU6050 (gyroBias, accelBias);
-          mpu6050.initMPU6050 ();
-          mpu6050.getAres ();
-          mpu6050.getGres ();
-          imu_ready = true;
-        }
-    }
-  else
-    {
-      setLEDs (1, 0, 1, 0);
-      wait_ms (1000);
-    }
-  // Read garbage data
-  while (!cam.integrationReady ())
-    ;
-  cam.read ();
-
-  while (1)
-    {
-      /***** Read line sensor *****/
-      while (!cam.integrationReady ())
-        ; // Wait for integration
-      cam.read ();
-      /***** Line following loop *****/
-
-      line_pos = cam.findLineEdge (LINE_THRESHOLD, LINE_PRECISION,
-                                   LINE_CROP_AMOUNT);
+  wait_ms(100);
+  uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);
 
-      if (line_pos != -1)
-        { // On the line
-          // Set LEDs based on the position of the line
-          if (line_pos < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) / 4)
-            {
-              setLEDs (1, 0, 0, 0);
-            }
-          else if (line_pos
-                   < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) / 2)
-            {
-              setLEDs (0, 1, 0, 0);
-            }
-          else if (line_pos
-                   < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) * 3 / 4)
-            {
-              setLEDs (0, 0, 1, 0);
-            }
-          else
-            {
-              setLEDs (0, 0, 0, 1);
-            }
+  if (whoami == 0x68)  // WHO_AM_I should always be 0x68
+  {
+    mpu6050.MPU6050SelfTest(SelfTest);
+    if (SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f &&
+        SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
+      mpu6050.resetMPU6050();
+      mpu6050.calibrateMPU6050(gyroBias, accelBias);
+      mpu6050.initMPU6050();
+      mpu6050.getAres();
+      mpu6050.getGres();
+      imu_ready = true;
+    }
+  } else {
+    setLEDs(1, 0, 1, 0);
+    wait_ms(10000);
+  }
+  // Read garbage data
+  while (!cam.integrationReady())
+    ;
+  cam.read();
 
-          // Record the line position
-          line_hist[hist_index] = line_pos;
-          line_lost_time = 0;
-          if (imu_ready)
-            {
-
-              if (mpu6050.readByte (MPU6050_ADDRESS, INT_STATUS) & 0x01)
-                { // check if data ready interrupt
-                  mpu6050.readAccelData (accelCount);
-                  // Read the x/y/z adc values
-
-                  accel_x[hist_index] = accelCount[0];
-                  accel_y[hist_index] = accelCount[1];
-                  accel_z[hist_index] = accelCount[2];
-
-                  // Calculate the gyro value into actual degrees per
-                  // second
-
-                  mpu6050.readGyroData (gyroCount);
+  while (1) {
+    /***** Read line sensor *****/
+    while (!cam.integrationReady()) {
+        setLEDs(1, 0, 0, 1);
+    }
+    cam.read();
+    /***** Line following loop *****/
 
-                  gyro_x[hist_index] = gyroCount[0];
-                  gyro_y[hist_index] = gyroCount[1];
-                  gyro_z[hist_index] = gyroCount[2];
-                  // Read the x/y/z ad values
-                  // gyro_x = (float)gyroCount[0] * gRes
-                  //          - gyroBias[0]; // get actual gyro value, this
-                  //                         // depends on scale being set
-                  // gyro_y = (float)gyroCount[1] * gRes - gyroBias[1];
-                  // gyro_z = (float)gyroCount[2] * gRes - gyroBias[2];
-                  //   pc.printf ("Gyro,%d,%d,%d\n", gyroCount[0],
-                  //   gyroCount[1],
-                  //              gyroCount[2]);
-                  //
-                }
-            }
-          hist_index++;
+    line_pos =
+        cam.findLineEdge(LINE_THRESHOLD, LINE_PRECISION, LINE_CROP_AMOUNT);
+    if (line_pos <= 8) {
+        line_pos = -1;
+    }
+    if (line_pos != -1) {  // On the line
+      // Set LEDs based on the position of the line
+      if (line_pos < (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) / 4) {
+        setLEDs(1, 0, 0, 0);
+      } else if (line_pos <
+                 (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) / 2) {
+        setLEDs(0, 1, 0, 0);
+      } else if (line_pos <
+                 (TSL1401CL_PIXEL_COUNT - 2 * LINE_CROP_AMOUNT) * 3 / 4) {
+        setLEDs(0, 0, 1, 0);
+      } else {
+        setLEDs(0, 0, 0, 1);
+      }
 
-          // Steer the vehicle
-          steerImprovedPointTurns (line_pos);
+      // Record the line position
+      line_hist[hist_index] = line_pos;
+      line_lost_time = 0;
+      hist_index++;
+
+      // Steer the vehicle
+      steerImprovedPointTurns(line_pos);
 
-          line_pos_previous = line_pos;
+      line_pos_previous = line_pos;
+    } else {  // Lost the line
+      setLEDs (1, 1, 1, 1);
+      if (abs(line_pos_previous - TSL1401CL_PIXEL_COUNT / 2) >
+          TSL1401CL_PIXEL_COUNT / 4) {
+        // Steer at maximum turn angle towards the last known line
+        // direction
+        if (line_pos_previous >= TSL1401CL_PIXEL_COUNT / 2) {
+          steerImprovedPointTurns(TSL1401CL_PIXEL_COUNT - 1 - LINE_CROP_AMOUNT);
+          // line_hist[hist_index] = TSL1401CL_PIXEL_COUNT - 1 -
+          // LINE_CROP_AMOUNT;
+        } else {
+          steerImprovedPointTurns(LINE_CROP_AMOUNT);
+          // line_hist[hist_index] = LINE_CROP_AMOUNT;
         }
-      else
-        { // Lost the line
-          // setLEDs (1, 1, 1, 1);
-          if (abs (line_pos_previous - TSL1401CL_PIXEL_COUNT / 2)
-              > TSL1401CL_PIXEL_COUNT / 4)
-            {
-              // Steer at maximum turn angle towards the last known line
-              // direction
-              if (line_pos_previous >= TSL1401CL_PIXEL_COUNT / 2)
-                {
-                  steerImprovedPointTurns (TSL1401CL_PIXEL_COUNT - 1
-                                           - LINE_CROP_AMOUNT);
-                  // line_hist[hist_index] = TSL1401CL_PIXEL_COUNT - 1 -
-                  // LINE_CROP_AMOUNT;
-                }
-              else
-                {
-                  steerImprovedPointTurns (LINE_CROP_AMOUNT);
-                  // line_hist[hist_index] = LINE_CROP_AMOUNT;
-                }
-            }
-          else
-            {
-              line_lost_time++;
-              if (line_lost_time >= LINE_END_TIME)
-                {
-                  setLEDs (1, 0, 1, 0);
-                  // Line end; transmit data to PC
-                  m1_fwd.write (0);
-                  m2_fwd.write (0);
-                  // while (!pc.readable ())
-                  //   ;
-                  pc.printf ("----- Start line data -----\n");
-                  for (int i = 0; i <= hist_index; i++)
-                    {
-                      pc.printf ("Pos: %d\n", line_hist[i]);
-                      float x, y, z;
-                      x = (float)accel_x[i] * aRes - accelBias[0];
-                      y = (float)accel_y[i] * aRes - accelBias[1];
-                      z = (float)accel_z[i] * aRes - accelBias[2];
-                      pc.printf ("Acceleration,%f,%f,%f\n", x, y, z);
-                      x = (float)gyro_x[i] * gRes - gyroBias[0];
-                      y = (float)gyro_y[i] * gRes - gyroBias[1];
-                      z = (float)gyro_z[i] * gRes - gyroBias[2];
-                      pc.printf ("Gyroscope,%f,%f,%f\n", x, y, z);
-                    }
-                  while (1)
-                    ;
-                }
-            }
-        }
-      if (hist_index > LINE_HIST_SIZE)
-        {
-          setLEDs (1, 0, 0, 1);
-
+      } else {
+        line_lost_time++;
+        if (line_lost_time >= LINE_END_TIME) {
+          setLEDs(1, 0, 1, 0);
           // Line end; transmit data to PC
-          m1_fwd.write (0);
-          m2_fwd.write (0);
-          // while (!pc.readable ())
+          m1_fwd.write(0);
+          m2_fwd.write(0);
+          m1_back.write(0);
+          m2_back.write(0);
+          // while (!pc.readable())
           //   ;
-          pc.printf ("----- Start line data -----\n");
-          for (int i = 0; i <= hist_index; i++)
-            {
-              pc.printf ("Pos: %d\n", line_hist[i]);
-              float x, y, z;
-              x = (float)accel_x[i] * aRes - accelBias[0];
-              y = (float)accel_y[i] * aRes - accelBias[1];
-              z = (float)accel_z[i] * aRes - accelBias[2];
-              pc.printf ("Acceleration,%f,%f,%f\n", x, y, z);
-              x = (float)gyro_x[i] * gRes - gyroBias[0];
-              y = (float)gyro_y[i] * gRes - gyroBias[1];
-              z = (float)gyro_z[i] * gRes - gyroBias[2];
-              pc.printf ("Gyroscope,%f,%f,%f\n", x, y, z);
-            }
           while (1)
             ;
         }
+      }
     }
+    
+    if (imu_ready) {
+      if (mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) &
+          0x01) {                           // check if data ready interrupt
+        mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
+        mpu6050.readGyroData(gyroCount);    // Read the x/y/z adc values
+        // Now we'll calculate the accleration value into actual g's
+        accel_x = (float)accelCount[0] * aRes -
+                  accelBias[0];  // get actual g value, this depends on scale
+                                 // being set
+        accel_y = (float)accelCount[1] * aRes - accelBias[1];
+        accel_z = (float)accelCount[2] * aRes - accelBias[2];
+                
+        if (accel_z < RAISE_ACCELERATION) {
+            pc.printf("%f\n", accel_z);
+            raise_time++;
+            if (raise_time > RAISE_THRESHOLD) {
+                setLEDs(1, 1, 1, 1);
+                int fall_time = 0;
+                m1_fwd.write(0);
+                m1_back.write(0);
+                m2_fwd.write(0);
+                m2_back.write(0);
+                while (fall_time < FALL_THRESHOLD) {
+                    if (mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) &
+                        0x01) {                           // check if data ready interrupt
+                        mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
+                        accel_z = (float)accelCount[2] * aRes - accelBias[2];
+                        if (accel_z > FALL_ACCELERATION) {
+                            pc.printf("%f\n", accel_z);
+                            fall_time++;
+                        } else {
+                            fall_time = 0;
+                        }
+                    }
+                }
+                setLEDs(0, 0, 0, 0);
+            }
+        } else {
+            raise_time = 0;
+        }
+        // Calculate the gyro value into actual degrees per second
+        gyro_x = (float)gyroCount[0] * gRes - gyroBias[0];  // get actual gyro
+                                                            // value, this
+                                                            // depends on scale
+                                                            // being set
+        gyro_y = (float)gyroCount[1] * gRes - gyroBias[1];
+        gyro_z = (float)gyroCount[2] * gRes - gyroBias[2];
+      }
+    }
+  }
 }
 
-float
-max (float a, float b)
-{
-  if (a >= b)
-    return a;
+float max(float a, float b) {
+  if (a >= b) return a;
   return b;
 }
 
-void
-setLEDs (uint8_t a, uint8_t b, uint8_t c, uint8_t d)
-{
+void setLEDs(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
   led1 = a;
   led2 = b;
   led3 = c;
   led4 = d;
 }
 
-void
-steerStupid (int8_t line_pos)
-{
-  line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
+void steerStupid(int8_t line_pos) {
+  line_pos -= TSL1401CL_PIXEL_COUNT / 2;  // Find offset from center
 
-  if (line_pos < 0)
-    {
-      m1_fwd.write (SPEED_PWM);
-      m1_back.write (0);
-      m2_fwd.write (max (SPEED_PWM * (1.0F
-                                      + TURN_SENS_INNER * line_pos * 2.0F
-                                            / (float)TSL1401CL_PIXEL_COUNT),
-                         0));
-      m2_back.write (0);
-    }
+  if (line_pos < 0) {
+    m1_fwd.write(SPEED_PWM);
+    m1_back.write(0);
+    m2_fwd.write(max(SPEED_PWM * (1.0F +
+                                  TURN_SENS_INNER * line_pos * 2.0F /
+                                      (float)TSL1401CL_PIXEL_COUNT),
+                     0));
+    m2_back.write(0);
+  }
 
-  if (line_pos >= 0)
-    {
-      m2_fwd.write (SPEED_PWM);
-      m2_back.write (0);
-      m1_fwd.write (max (SPEED_PWM * (1.0F
-                                      - TURN_SENS_INNER * line_pos * 2.0F
-                                            / (float)TSL1401CL_PIXEL_COUNT),
-                         0));
-      m1_back.write (0);
-    }
+  if (line_pos >= 0) {
+    m2_fwd.write(SPEED_PWM);
+    m2_back.write(0);
+    m1_fwd.write(max(SPEED_PWM * (1.0F -
+                                  TURN_SENS_INNER * line_pos * 2.0F /
+                                      (float)TSL1401CL_PIXEL_COUNT),
+                     0));
+    m1_back.write(0);
+  }
 }
 
-void
-steerPointTurns (int8_t line_pos)
-{
-  line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
+void steerPointTurns(int8_t line_pos) {
+  line_pos -= TSL1401CL_PIXEL_COUNT / 2;  // Find offset from center
 
   float pwm_outer = SPEED_PWM;
-  float pwm_inner = SPEED_PWM * (1.0F
-                                 - TURN_SENS_INNER * abs (line_pos) * 2.0F
-                                       / (float)TSL1401CL_PIXEL_COUNT);
+  float pwm_inner = SPEED_PWM * (1.0F -
+                                 TURN_SENS_INNER * abs(line_pos) * 2.0F /
+                                     (float)TSL1401CL_PIXEL_COUNT);
 
-  if (line_pos < 0)
-    {
-      m1_fwd.write (pwm_outer);
-      m1_back.write (0);
-      if (pwm_inner >= 0)
-        {
-          m2_fwd.write (pwm_inner);
-          m2_back.write (0);
-        }
-      else
-        {
-          m2_fwd.write (0);
-          m2_back.write (pwm_inner * -1.0F);
-        }
+  if (line_pos < 0) {
+    m1_fwd.write(pwm_outer);
+    m1_back.write(0);
+    if (pwm_inner >= 0) {
+      m2_fwd.write(pwm_inner);
+      m2_back.write(0);
+    } else {
+      m2_fwd.write(0);
+      m2_back.write(pwm_inner * -1.0F);
     }
+  }
 
-  if (line_pos >= 0)
-    {
-      m2_fwd.write (pwm_outer);
-      m2_back.write (0);
-      if (pwm_inner >= 0)
-        {
-          m1_fwd.write (pwm_inner);
-          m1_back.write (0);
-        }
-      else
-        {
-          m1_fwd.write (0);
-          m1_back.write (pwm_inner * -1.0F);
-        }
+  if (line_pos >= 0) {
+    m2_fwd.write(pwm_outer);
+    m2_back.write(0);
+    if (pwm_inner >= 0) {
+      m1_fwd.write(pwm_inner);
+      m1_back.write(0);
+    } else {
+      m1_fwd.write(0);
+      m1_back.write(pwm_inner * -1.0F);
     }
+  }
 }
 
-void
-steerImprovedPointTurns (int8_t line_pos)
-{
-  line_pos -= TSL1401CL_PIXEL_COUNT / 2; // Find offset from center
+void steerImprovedPointTurns(int8_t line_pos) {
+  line_pos -= TSL1401CL_PIXEL_COUNT / 2;  // Find offset from center
 
   // Find desired motor voltages based on the controller
-  float pwm_outer = SPEED_PWM * (1.0F
-                                 + TURN_SENS_OUTER * abs (line_pos) * 2.0F
-                                       / (float)TSL1401CL_PIXEL_COUNT);
-  float pwm_inner = SPEED_PWM * (1.0F
-                                 - TURN_SENS_INNER * abs (line_pos) * 2.0F
-                                       / (float)TSL1401CL_PIXEL_COUNT);
+  float pwm_outer = SPEED_PWM * (1.0F +
+                                 TURN_SENS_OUTER * abs(line_pos) * 2.0F /
+                                     (float)TSL1401CL_PIXEL_COUNT);
+  float pwm_inner = SPEED_PWM * (1.0F -
+                                 TURN_SENS_INNER * abs(line_pos) * 2.0F /
+                                     (float)TSL1401CL_PIXEL_COUNT);
 
   // Write to the appropriate PWM pins
-  if (line_pos < 0)
-    {
-      m1_fwd.write (pwm_outer);
-      m1_back.write (0);
-      if (pwm_inner >= 0)
-        {
-          m2_fwd.write (pwm_inner);
-          m2_back.write (0);
-        }
-      else
-        {
-          m2_fwd.write (0);
-          m2_back.write (pwm_inner * -1.0F);
-        }
+  if (line_pos < 0) {
+    m1_fwd.write(pwm_outer);
+    m1_back.write(0);
+    if (pwm_inner >= 0) {
+      m2_fwd.write(pwm_inner);
+      m2_back.write(0);
+    } else {
+      m2_fwd.write(0);
+      //m2_back.write(pwm_inner * -1.0F);
     }
+  }
 
-  if (line_pos >= 0)
-    {
-      m2_fwd.write (pwm_outer);
-      m2_back.write (0);
-      if (pwm_inner >= 0)
-        {
-          m1_fwd.write (pwm_inner);
-          m1_back.write (0);
-        }
-      else
-        {
-          m1_fwd.write (0);
-          m1_back.write (pwm_inner * -1.0F);
-        }
+  if (line_pos >= 0) {
+    m2_fwd.write(pwm_outer);
+    m2_back.write(0);
+    if (pwm_inner >= 0) {
+      m1_fwd.write(pwm_inner);
+      m1_back.write(0);
+    } else {
+      m1_fwd.write(0);
+      //m1_back.write(pwm_inner * -1.0F);
     }
+  }
 }
\ No newline at end of file