Revision 40:e79cefc241f8, committed 2015-04-14

Comitter:

DavidEGrayson

Date:

Tue Apr 14 01:45:01 2015 +0000

Parent:

39:a5e25fd52ff8

Child:

41:4a4be80ff38c

Commit message:

added that derivative term;

Changed in this revision

--- a/line_sensors.cpp	Tue Apr 14 01:06:41 2015 +0000
+++ b/line_sensors.cpp	Tue Apr 14 01:45:01 2015 +0000
@@ -1,14 +1,5 @@
 #include "line_sensors.h"
 
-
-/**
-AnalogIn lineSensorsAnalog[LINE_SENSOR_COUNT] = {
-    AnalogIn(p20), // brown wire, left-most sensor
-    AnalogIn(p19), // orange wire, middle sensor
-    AnalogIn(p17), // blue wire, right-most sensor
-};  // TODO: remove
-**/
-
 DigitalInOut lineSensorsDigital[LINE_SENSOR_COUNT] = {
     DigitalInOut(p18), // white wire, left-most sensor
     DigitalInOut(p19), // orange wire, middle sensor
@@ -27,14 +18,14 @@
     
     wait_us(10);
     
-    Timer timer;
-    timer.start();
-
     for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)
     {
         lineSensorsDigital[i].input();
     }
 
+    Timer timer;
+    timer.start();
+
     while(timer.read_us() < 1000)
     {
         for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++)

--- a/main.cpp	Tue Apr 14 01:06:41 2015 +0000
+++ b/main.cpp	Tue Apr 14 01:45:01 2015 +0000
@@ -58,7 +58,7 @@
     setLeds(1, 0, 0, 0);
     waitForSignalToStart();
         
-    setLeds(0, 1, 0, 0);    
+    setLeds(0, 1, 0, 0);   
     followLineFast();
     
     setLeds(1, 1, 1, 1);
@@ -181,7 +181,7 @@
 void updateMotorsToFollowLine()
 {
     const int16_t drivingSpeed = 400;
-    const int followLineStrength = drivingSpeed * 5 / 4;
+    const int32_t followLineStrength = drivingSpeed * 5 / 4;
 
     int16_t speedLeft = drivingSpeed;
     int16_t speedRight = drivingSpeed;
@@ -201,21 +201,21 @@
 void updateMotorsToFollowLineFast()
 {
     const int16_t drivingSpeed = 600;
-    const int followLineStrength = drivingSpeed * 5 / 4;
-    static int16_t lastPosition = 0;
+    const int32_t followLineStrength = drivingSpeed * 5 / 4;
+    static int16_t lastPosition = 1000;
 
     int16_t position = lineTracker.getLinePosition();
 
     int16_t speedLeft = drivingSpeed;
     int16_t speedRight = drivingSpeed;
-    int16_t reduction = (position - 1000) * followLineStrength / 1000;
-    if(reduction < 0)
+    int32_t veer = (position - 1000) * followLineStrength / 1000 + (position - lastPosition) * 200;
+    if(veer > 0)
     {
-        speedLeft = reduceSpeed(speedLeft, -reduction);   
+        speedRight = reduceSpeed(speedRight, veer);
     }
     else
     {
-        speedRight = reduceSpeed(speedRight, reduction);
+        speedLeft = reduceSpeed(speedLeft, -veer);   
     }
     
     motorsSpeedSet(speedLeft, speedRight);
@@ -228,11 +228,20 @@
     Pacer reportPacer(200000);
     
     loadCalibration();
+    uint32_t loopCount = 0;
+    Timer timer;
+    timer.start();
     while(1)
     {
+        loopCount += 1;
         updateReckonerFromEncoders();
         loggerService();
-
+        
+        if ((loopCount % 256) == 0)
+        {
+            pc.printf("%d\r\n", lineTracker.getLinePosition());
+        }
+        
         lineTracker.read();
         updateMotorsToFollowLineFast();        
     }

--- a/motors.cpp	Tue Apr 14 01:06:41 2015 +0000
+++ b/motors.cpp	Tue Apr 14 01:45:01 2015 +0000
@@ -62,7 +62,6 @@
     {
         motorLeftDir = 1;
     }
-    LPC_PWM1->MR1 = motorLeftSpeed;
     
     if (motorRightSpeed < 0)
     {
@@ -73,6 +72,7 @@
     {
         motorRightDir = 1;
     }
+    LPC_PWM1->MR1 = motorLeftSpeed;
     LPC_PWM1->MR3 = motorRightSpeed;
     
     LPC_PWM1->LER |= (1<<1) | (1<<3);