Diff: main.cpp

Revision:

1:f3aee4ff2b19

Parent:

0:4a43061bfa34

--- a/main.cpp	Tue Aug 25 02:50:22 2015 +0000
+++ b/main.cpp	Thu Aug 27 01:42:49 2015 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 #include "QTRSensors.h"
-#define NUM_SENSORS 7
+#define NUM_SENSORS 6
 Serial pc(USBTX, USBRX);
 
 DigitalOut myled(LED1);
@@ -8,105 +8,130 @@
 // create an object for four QTR-xRC sensors on digital pins 0 and 9, and on analog
 // inputs 1 and 3 (which are being used as digital inputs 15 and 17 in this case)
 
-QTRSensorsAnalog qtra((PinName[]) {A5, A4, A3, A2, A1, A0, PA_3}, NUM_SENSORS, 4, D7);
+QTRSensorsAnalog qtra((PinName[])
+{
+    A5, A4, A3, A2, A1, A0
+}, NUM_SENSORS, 4, D7);
 // PA_3, PA_2, PA_10, PB_3, PB_5, PB_4, PB_10
 
 void setup(void);
 void readSensors(void);
 void readSensorsPlain(void);
 
-int main() {
+int last_proportional;
+int integral;
+
+int main()
+{
     pc.printf("Setup!\r\n");
     setup();
     pc.printf("Start Loop!\r\n");
     while(1) {
         pc.printf("Read Sensors!\r\n");
-        readSensorsPlain();
+        readSensors();
     }
 }
 
 void setup()
 {
-  // optional: wait for some input from the user, such as  a button press
- 
-  // then start calibration phase and move the sensors over both
-  // reflectance extremes they will encounter in your application:
-  int i;
-  for (i = 0; i < 50; i++)  // make the calibration take about 5 seconds
-  // originally i < 250
-  {
-    pc.printf("Calibrate Session %d\r\n",i);
-    qtra.calibrate(QTR_EMITTERS_ON);
-    wait_ms(20);
-  }
-  
-  for (i = 0; i < NUM_SENSORS; i++)
-  {
-      pc.printf("Min On %d \r\n", qtra.calibratedMinimumOn[i]);
-  }
- 
-  // optional: signal that the calibration phase is now over and wait for further
-  // input from the user, such as a button press
+    last_proportional = 0;
+    integral = 0;
+    // optional: wait for some input from the user, such as  a button press
+
+    // then start calibration phase and move the sensors over both
+    // reflectance extremes they will encounter in your application:
+    int i;
+    for (i = 0; i < 40; i++)  // make the calibration take about 5 seconds
+        // originally i < 250
+    {
+        pc.printf("Calibrate Session %d\r\n",i);
+        qtra.calibrate(QTR_EMITTERS_ON);
+        wait_ms(20);
+    }
+
+    for (i = 0; i < NUM_SENSORS; i++) {
+        pc.printf("Min On %d \r\n", qtra.calibratedMinimumOn[i]);
+    }
+
+    // optional: signal that the calibration phase is now over and wait for further
+    // input from the user, such as a button press
 }
 
 void readSensors()
 {
-  unsigned int sensors[NUM_SENSORS];
-  // get calibrated sensor values returned in the sensors array, along with the line position
-  // position will range from 0 to 2000, with 1000 corresponding to the line over the middle 
-  // sensor.
-  int position = qtra.readLine(sensors);
- 
-  // if all three sensors see very low reflectance, take some appropriate action for this 
-  // situation.
-  if (sensors[0] > 750 && sensors[1] > 750 && sensors[2] > 750)
-  {
-    // do something.  Maybe this means we're at the edge of a course or about to fall off 
-    // a table, in which case, we might want to stop moving, back up, and turn around.
-    pc.printf("do something.  Maybe this means we're at the edge of a course or about to fall off a table, in which case, we might want to stop moving, back up, and turn around.");
+    unsigned int sensors[NUM_SENSORS];
+    // get calibrated sensor values returned in the sensors array, along with the line position
+    // position will range from 0 to 6000, with 3000 corresponding to the line over the middle
+    // sensor.
+    
+    int position = qtra.readLine(sensors, QTR_EMITTERS_ON);
     
-    //return;
-  }
- 
-  // compute our "error" from the line position.  We will make it so that the error is zero 
-  // when the middle sensor is over the line, because this is our goal.  Error will range from
-  // -1000 to +1000.  If we have sensor 0 on the left and sensor 2 on the right,  a reading of 
-  // -1000 means that we see the line on the left and a reading of +1000 means we see the 
-  // line on the right.
-  int error = position - 1000;
- 
-  int leftMotorSpeed = 200;
-  int rightMotorSpeed = 200;
-  if (error < -500)  // the line is on the left
-    leftMotorSpeed = 20;  // turn left
-  if (error > 500)  // the line is on the right
-    rightMotorSpeed = 20;  // turn right
+    pc.printf("Sensor Values: ");
+    unsigned char i = 0;
+    for (i = 0; i < NUM_SENSORS; i++)
+    {
+        pc.printf(" %d, ", sensors[i]);
+    }
+    pc.printf("\r\n");
+
+    // if all three sensors see very low reflectance, take some appropriate action for this
+    // situation.
+    if (sensors[0] > 750 && sensors[3] > 750 && sensors[5] > 750) {
+        // do something.  Maybe this means we're at the edge of a course or about to fall off
+        // a table, in which case, we might want to stop moving, back up, and turn around.
+        pc.printf("do something.  Maybe this means we're at the edge of a course or about to fall off a table, in which case, we might want to stop moving, back up, and turn around.");
+
+        //return;
+    }
 
-  pc.printf("Left Motor %d, Right Motor %d\r\n", leftMotorSpeed, rightMotorSpeed);
-          myled = 1; // LED is ON
-        wait_ms(leftMotorSpeed); // 200 ms
-        myled = 0; // LED is OFF
-        wait(rightMotorSpeed); // 1 sec
+    // compute our "error" from the line position.  We will make it so that the error is zero
+    // when the middle sensor is over the line, because this is our goal.  Error will range from
+    // -1000 to +1000.  If we have sensor 0 on the left and sensor 2 on the right,  a reading of
+    // -1000 means that we see the line on the left and a reading of +1000 means we see the
+    // line on the right.
+    int error = position - 3000;
+    int proportional = error / 25;
+    int derivative = proportional - last_proportional;
+    last_proportional = proportional;
+    integral += proportional;
+    if (integral > 12000 || integral < -12000) {
+        integral = 0;
+    }
+    int motor_diff = proportional + (integral / 1000) + derivative * 2;
 
- 
-  // set motor speeds using the two motor speed variables above
+    int leftMotorSpeed = 200;
+    int rightMotorSpeed = 200;
+    if (motor_diff > 200)
+        motor_diff = 200;
+    else if (motor_diff < -200)
+        motor_diff = -200;
+        
+    if (motor_diff < 0)  // the line is on the left
+        leftMotorSpeed += motor_diff;  // turn left
+    else 
+        rightMotorSpeed -= motor_diff;  // turn right
+
+    pc.printf("Position: %d, Error: %d, Proportional %d, Integral %d, Derivative %d, Motor Diff %d: %d : %d\r\n", position, error, proportional, integral, derivative, motor_diff, leftMotorSpeed, rightMotorSpeed);
+    wait_ms(50);
+
+    // set motor speeds using the two motor speed variables above
 }
 
 
 void readSensorsPlain()
 {
-  unsigned int sensors[NUM_SENSORS];
-  // get calibrated sensor values returned in the sensors array, along with the line position
-  // position will range from 0 to 2000, with 1000 corresponding to the line over the middle 
-  // sensor.
-  pc.printf("before\r\n");
-  qtra.readCalibrated(sensors, QTR_EMITTERS_ON);
-  pc.printf("after\r\n");
-  
-  for (unsigned int i = 0; i < NUM_SENSORS; i++) {
- 
-      pc.printf("Sensor Value[%d] %d  Min %d    Max %d \r\n", i, sensors[i], qtra.calibratedMinimumOn[i], qtra.calibratedMaximumOn[i]);
-    
-  } 
-  // set motor speeds using the two motor speed variables above
+    unsigned int sensors[NUM_SENSORS];
+    // get calibrated sensor values returned in the sensors array, along with the line position
+    // position will range from 0 to 2000, with 1000 corresponding to the line over the middle
+    // sensor.
+    pc.printf("before\r\n");
+    qtra.readCalibrated(sensors, QTR_EMITTERS_ON);
+    pc.printf("after\r\n");
+
+    for (unsigned int i = 0; i < NUM_SENSORS; i++) {
+
+        pc.printf("Sensor Value[%d] %d  Min %d    Max %d \r\n", i, sensors[i], qtra.calibratedMinimumOn[i], qtra.calibratedMaximumOn[i]);
+
+    }
+    // set motor speeds using the two motor speed variables above
 }
\ No newline at end of file