main.cpp

00001 #include "mbed.h"
00002 #include "QTRSensors.h"
00003 #define NUM_SENSORS 6
00004 Serial pc(USBTX, USBRX);
00005 
00006 DigitalOut myled(LED1);
00007 
00008 // create an object for four QTR-xRC sensors on digital pins 0 and 9, and on analog
00009 // inputs 1 and 3 (which are being used as digital inputs 15 and 17 in this case)
00010 
00011 QTRSensorsAnalog qtra((PinName[])
00012 {
00013     A5, A4, A3, A2, A1, A0
00014 }, NUM_SENSORS, 4, D7);
00015 // PA_3, PA_2, PA_10, PB_3, PB_5, PB_4, PB_10
00016 
00017 void setup(void);
00018 void readSensors(void);
00019 void readSensorsPlain(void);
00020 
00021 int last_proportional;
00022 int integral;
00023 
00024 int main()
00025 {
00026     pc.printf("Setup!\r\n");
00027     setup();
00028     pc.printf("Start Loop!\r\n");
00029     while(1) {
00030         pc.printf("Read Sensors!\r\n");
00031         readSensors();
00032     }
00033 }
00034 
00035 void setup()
00036 {
00037     last_proportional = 0;
00038     integral = 0;
00039     // optional: wait for some input from the user, such as  a button press
00040 
00041     // then start calibration phase and move the sensors over both
00042     // reflectance extremes they will encounter in your application:
00043     int i;
00044     for (i = 0; i < 40; i++)  // make the calibration take about 5 seconds
00045         // originally i < 250
00046     {
00047         pc.printf("Calibrate Session %d\r\n",i);
00048         qtra.calibrate(QTR_EMITTERS_ON);
00049         wait_ms(20);
00050     }
00051 
00052     for (i = 0; i < NUM_SENSORS; i++) {
00053         pc.printf("Min On %d \r\n", qtra.calibratedMinimumOn[i]);
00054     }
00055 
00056     // optional: signal that the calibration phase is now over and wait for further
00057     // input from the user, such as a button press
00058 }
00059 
00060 void readSensors()
00061 {
00062     unsigned int sensors[NUM_SENSORS];
00063     // get calibrated sensor values returned in the sensors array, along with the line position
00064     // position will range from 0 to 6000, with 3000 corresponding to the line over the middle
00065     // sensor.
00066     
00067     int position = qtra.readLine(sensors, QTR_EMITTERS_ON);
00068     
00069     pc.printf("Sensor Values: ");
00070     unsigned char i = 0;
00071     for (i = 0; i < NUM_SENSORS; i++)
00072     {
00073         pc.printf(" %d, ", sensors[i]);
00074     }
00075     pc.printf("\r\n");
00076 
00077     // if all three sensors see very low reflectance, take some appropriate action for this
00078     // situation.
00079     if (sensors[0] > 750 && sensors[3] > 750 && sensors[5] > 750) {
00080         // do something.  Maybe this means we're at the edge of a course or about to fall off
00081         // a table, in which case, we might want to stop moving, back up, and turn around.
00082         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.");
00083 
00084         //return;
00085     }
00086 
00087     // compute our "error" from the line position.  We will make it so that the error is zero
00088     // when the middle sensor is over the line, because this is our goal.  Error will range from
00089     // -1000 to +1000.  If we have sensor 0 on the left and sensor 2 on the right,  a reading of
00090     // -1000 means that we see the line on the left and a reading of +1000 means we see the
00091     // line on the right.
00092     int error = position - 3000;
00093     int proportional = error / 25;
00094     int derivative = proportional - last_proportional;
00095     last_proportional = proportional;
00096     integral += proportional;
00097     if (integral > 12000 || integral < -12000) {
00098         integral = 0;
00099     }
00100     int motor_diff = proportional + (integral / 1000) + derivative * 2;
00101 
00102     int leftMotorSpeed = 200;
00103     int rightMotorSpeed = 200;
00104     if (motor_diff > 200)
00105         motor_diff = 200;
00106     else if (motor_diff < -200)
00107         motor_diff = -200;
00108         
00109     if (motor_diff < 0)  // the line is on the left
00110         leftMotorSpeed += motor_diff;  // turn left
00111     else 
00112         rightMotorSpeed -= motor_diff;  // turn right
00113 
00114     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);
00115     wait_ms(50);
00116 
00117     // set motor speeds using the two motor speed variables above
00118 }
00119 
00120 
00121 void readSensorsPlain()
00122 {
00123     unsigned int sensors[NUM_SENSORS];
00124     // get calibrated sensor values returned in the sensors array, along with the line position
00125     // position will range from 0 to 2000, with 1000 corresponding to the line over the middle
00126     // sensor.
00127     pc.printf("before\r\n");
00128     qtra.readCalibrated(sensors, QTR_EMITTERS_ON);
00129     pc.printf("after\r\n");
00130 
00131     for (unsigned int i = 0; i < NUM_SENSORS; i++) {
00132 
00133         pc.printf("Sensor Value[%d] %d  Min %d    Max %d \r\n", i, sensors[i], qtra.calibratedMinimumOn[i], qtra.calibratedMaximumOn[i]);
00134 
00135     }
00136     // set motor speeds using the two motor speed variables above
00137 }