Matthew Phillipps / Mbed 2 deprecated Nucleo_QTR

Test Harness for Pololu QTR Library

Dependencies:   PololuQTRSensors mbed

main.cpp@0:4a43061bfa34, 2015-08-25 (annotated)

Committer:
phillippsm
Date:
Tue Aug 25 02:50:22 2015 +0000
Revision:
0:4a43061bfa34
Child:
1:f3aee4ff2b19
Alpha Release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
phillippsm 0:4a43061bfa34 1 #include "mbed.h"
phillippsm 0:4a43061bfa34 2 #include "QTRSensors.h"
phillippsm 0:4a43061bfa34 3 #define NUM_SENSORS 7
phillippsm 0:4a43061bfa34 4 Serial pc(USBTX, USBRX);
phillippsm 0:4a43061bfa34 5
phillippsm 0:4a43061bfa34 6 DigitalOut myled(LED1);
phillippsm 0:4a43061bfa34 7
phillippsm 0:4a43061bfa34 8 // create an object for four QTR-xRC sensors on digital pins 0 and 9, and on analog
phillippsm 0:4a43061bfa34 9 // inputs 1 and 3 (which are being used as digital inputs 15 and 17 in this case)
phillippsm 0:4a43061bfa34 10
phillippsm 0:4a43061bfa34 11 QTRSensorsAnalog qtra((PinName[]) {A5, A4, A3, A2, A1, A0, PA_3}, NUM_SENSORS, 4, D7);
phillippsm 0:4a43061bfa34 12 // PA_3, PA_2, PA_10, PB_3, PB_5, PB_4, PB_10
phillippsm 0:4a43061bfa34 13
phillippsm 0:4a43061bfa34 14 void setup(void);
phillippsm 0:4a43061bfa34 15 void readSensors(void);
phillippsm 0:4a43061bfa34 16 void readSensorsPlain(void);
phillippsm 0:4a43061bfa34 17
phillippsm 0:4a43061bfa34 18 int main() {
phillippsm 0:4a43061bfa34 19 pc.printf("Setup!\r\n");
phillippsm 0:4a43061bfa34 20 setup();
phillippsm 0:4a43061bfa34 21 pc.printf("Start Loop!\r\n");
phillippsm 0:4a43061bfa34 22 while(1) {
phillippsm 0:4a43061bfa34 23 pc.printf("Read Sensors!\r\n");
phillippsm 0:4a43061bfa34 24 readSensorsPlain();
phillippsm 0:4a43061bfa34 25 }
phillippsm 0:4a43061bfa34 26 }
phillippsm 0:4a43061bfa34 27
phillippsm 0:4a43061bfa34 28 void setup()
phillippsm 0:4a43061bfa34 29 {
phillippsm 0:4a43061bfa34 30 // optional: wait for some input from the user, such as a button press
phillippsm 0:4a43061bfa34 31
phillippsm 0:4a43061bfa34 32 // then start calibration phase and move the sensors over both
phillippsm 0:4a43061bfa34 33 // reflectance extremes they will encounter in your application:
phillippsm 0:4a43061bfa34 34 int i;
phillippsm 0:4a43061bfa34 35 for (i = 0; i < 50; i++) // make the calibration take about 5 seconds
phillippsm 0:4a43061bfa34 36 // originally i < 250
phillippsm 0:4a43061bfa34 37 {
phillippsm 0:4a43061bfa34 38 pc.printf("Calibrate Session %d\r\n",i);
phillippsm 0:4a43061bfa34 39 qtra.calibrate(QTR_EMITTERS_ON);
phillippsm 0:4a43061bfa34 40 wait_ms(20);
phillippsm 0:4a43061bfa34 41 }
phillippsm 0:4a43061bfa34 42
phillippsm 0:4a43061bfa34 43 for (i = 0; i < NUM_SENSORS; i++)
phillippsm 0:4a43061bfa34 44 {
phillippsm 0:4a43061bfa34 45 pc.printf("Min On %d \r\n", qtra.calibratedMinimumOn[i]);
phillippsm 0:4a43061bfa34 46 }
phillippsm 0:4a43061bfa34 47
phillippsm 0:4a43061bfa34 48 // optional: signal that the calibration phase is now over and wait for further
phillippsm 0:4a43061bfa34 49 // input from the user, such as a button press
phillippsm 0:4a43061bfa34 50 }
phillippsm 0:4a43061bfa34 51
phillippsm 0:4a43061bfa34 52 void readSensors()
phillippsm 0:4a43061bfa34 53 {
phillippsm 0:4a43061bfa34 54 unsigned int sensors[NUM_SENSORS];
phillippsm 0:4a43061bfa34 55 // get calibrated sensor values returned in the sensors array, along with the line position
phillippsm 0:4a43061bfa34 56 // position will range from 0 to 2000, with 1000 corresponding to the line over the middle
phillippsm 0:4a43061bfa34 57 // sensor.
phillippsm 0:4a43061bfa34 58 int position = qtra.readLine(sensors);
phillippsm 0:4a43061bfa34 59
phillippsm 0:4a43061bfa34 60 // if all three sensors see very low reflectance, take some appropriate action for this
phillippsm 0:4a43061bfa34 61 // situation.
phillippsm 0:4a43061bfa34 62 if (sensors[0] > 750 && sensors[1] > 750 && sensors[2] > 750)
phillippsm 0:4a43061bfa34 63 {
phillippsm 0:4a43061bfa34 64 // do something. Maybe this means we're at the edge of a course or about to fall off
phillippsm 0:4a43061bfa34 65 // a table, in which case, we might want to stop moving, back up, and turn around.
phillippsm 0:4a43061bfa34 66 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.");
phillippsm 0:4a43061bfa34 67
phillippsm 0:4a43061bfa34 68 //return;
phillippsm 0:4a43061bfa34 69 }
phillippsm 0:4a43061bfa34 70
phillippsm 0:4a43061bfa34 71 // compute our "error" from the line position. We will make it so that the error is zero
phillippsm 0:4a43061bfa34 72 // when the middle sensor is over the line, because this is our goal. Error will range from
phillippsm 0:4a43061bfa34 73 // -1000 to +1000. If we have sensor 0 on the left and sensor 2 on the right, a reading of
phillippsm 0:4a43061bfa34 74 // -1000 means that we see the line on the left and a reading of +1000 means we see the
phillippsm 0:4a43061bfa34 75 // line on the right.
phillippsm 0:4a43061bfa34 76 int error = position - 1000;
phillippsm 0:4a43061bfa34 77
phillippsm 0:4a43061bfa34 78 int leftMotorSpeed = 200;
phillippsm 0:4a43061bfa34 79 int rightMotorSpeed = 200;
phillippsm 0:4a43061bfa34 80 if (error < -500) // the line is on the left
phillippsm 0:4a43061bfa34 81 leftMotorSpeed = 20; // turn left
phillippsm 0:4a43061bfa34 82 if (error > 500) // the line is on the right
phillippsm 0:4a43061bfa34 83 rightMotorSpeed = 20; // turn right
phillippsm 0:4a43061bfa34 84
phillippsm 0:4a43061bfa34 85 pc.printf("Left Motor %d, Right Motor %d\r\n", leftMotorSpeed, rightMotorSpeed);
phillippsm 0:4a43061bfa34 86 myled = 1; // LED is ON
phillippsm 0:4a43061bfa34 87 wait_ms(leftMotorSpeed); // 200 ms
phillippsm 0:4a43061bfa34 88 myled = 0; // LED is OFF
phillippsm 0:4a43061bfa34 89 wait(rightMotorSpeed); // 1 sec
phillippsm 0:4a43061bfa34 90
phillippsm 0:4a43061bfa34 91
phillippsm 0:4a43061bfa34 92 // set motor speeds using the two motor speed variables above
phillippsm 0:4a43061bfa34 93 }
phillippsm 0:4a43061bfa34 94
phillippsm 0:4a43061bfa34 95
phillippsm 0:4a43061bfa34 96 void readSensorsPlain()
phillippsm 0:4a43061bfa34 97 {
phillippsm 0:4a43061bfa34 98 unsigned int sensors[NUM_SENSORS];
phillippsm 0:4a43061bfa34 99 // get calibrated sensor values returned in the sensors array, along with the line position
phillippsm 0:4a43061bfa34 100 // position will range from 0 to 2000, with 1000 corresponding to the line over the middle
phillippsm 0:4a43061bfa34 101 // sensor.
phillippsm 0:4a43061bfa34 102 pc.printf("before\r\n");
phillippsm 0:4a43061bfa34 103 qtra.readCalibrated(sensors, QTR_EMITTERS_ON);
phillippsm 0:4a43061bfa34 104 pc.printf("after\r\n");
phillippsm 0:4a43061bfa34 105
phillippsm 0:4a43061bfa34 106 for (unsigned int i = 0; i < NUM_SENSORS; i++) {
phillippsm 0:4a43061bfa34 107
phillippsm 0:4a43061bfa34 108 pc.printf("Sensor Value[%d] %d Min %d Max %d \r\n", i, sensors[i], qtra.calibratedMinimumOn[i], qtra.calibratedMaximumOn[i]);
phillippsm 0:4a43061bfa34 109
phillippsm 0:4a43061bfa34 110 }
phillippsm 0:4a43061bfa34 111 // set motor speeds using the two motor speed variables above
phillippsm 0:4a43061bfa34 112 }