Diff: main.cpp

Revision:

0:4a43061bfa34

Child:

1:f3aee4ff2b19

diff -r 000000000000 -r 4a43061bfa34 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Aug 25 02:50:22 2015 +0000
@@ -0,0 +1,112 @@
+#include "mbed.h"
+#include "QTRSensors.h"
+#define NUM_SENSORS 7
+Serial pc(USBTX, USBRX);
+
+DigitalOut myled(LED1);
+
+// 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);
+// 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() {
+    pc.printf("Setup!\r\n");
+    setup();
+    pc.printf("Start Loop!\r\n");
+    while(1) {
+        pc.printf("Read Sensors!\r\n");
+        readSensorsPlain();
+    }
+}
+
+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
+}
+
+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.");
+    
+    //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("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
+
+ 
+  // 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
+}
\ No newline at end of file