![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Test Harness for Pololu QTR Library
Dependencies: PololuQTRSensors mbed
Revision 1:f3aee4ff2b19, committed 2015-08-27
- Comitter:
- phillippsm
- Date:
- Thu Aug 27 01:42:49 2015 +0000
- Parent:
- 0:4a43061bfa34
- Commit message:
- Alpha 0.1 - Changes to timings and removing redundant code
Changed in this revision
PololuQTRSensors.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/PololuQTRSensors.lib Tue Aug 25 02:50:22 2015 +0000 +++ b/PololuQTRSensors.lib Thu Aug 27 01:42:49 2015 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/users/phillippsm/code/PololuQTRSensors/#d54bb6a949bf +https://developer.mbed.org/users/phillippsm/code/PololuQTRSensors/#a664ab7aba8d
--- 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