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?
User | Revision | Line number | New 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 | } |