Matthew Phillipps / Mbed 2 deprecated Nucleo_QTR

Test Harness for Pololu QTR Library

Dependencies:   PololuQTRSensors mbed

main.cpp@1:f3aee4ff2b19, 2015-08-27 (annotated)

Committer:
phillippsm
Date:
Thu Aug 27 01:42:49 2015 +0000
Revision:
1:f3aee4ff2b19
Parent:
0:4a43061bfa34 
Alpha 0.1 - Changes to timings and removing redundant code

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