Diff: main.cpp

Revision:

0:76bca1f580a0

diff -r 000000000000 -r 76bca1f580a0 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 01 22:21:38 2016 +0000
@@ -0,0 +1,123 @@
+#include "scanner.h"
+#include "StepperDrive.h"
+#include "LongRangeSensor.h"
+#include "stdint.h"
+#include "mbed.h"
+
+Serial pc(USBTX, USBRX);
+Serial bluetooth(PTE16,PTE17); // bluetooth
+InterruptIn start(SW1);
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+bool active = false;
+
+void activate()
+{
+    led_red = 1;
+    led_green = 0;
+    active = true;
+}
+
+int main() 
+{
+    pc.baud(115200);
+    bluetooth.baud(9600);  /* interface via Bluetooth at 9600 */
+    start.mode(PullUp); /* Start/Stop button is active low needing PullUp */
+    start.fall(&activate);
+
+    led_red = 0;
+    led_green = 1;
+
+    StepperDrive drive(pc, PTE19, PTE18, 1, PTE3, PTE2, 0, 10.0625, 8.4800, 650); // 8.375
+              //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)
+
+    LongRangeSensor longRangeL(pc, PTB2);
+    LongRangeSensor longRangeR(pc, PTB3);
+
+    Scanner scanner(pc, drive, PTB0, PTB1, longRangeL, longRangeR, 1.0); // original period = 0.075
+        // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period)
+    
+    pc.printf("\nWaiting for START BUTTON\n");
+    bluetooth.printf("\nWaiting for START BUTTON\n");
+    while(!active) // wait for start_button
+    {
+        wait(1e-6);
+    }
+              
+    pc.printf("\nBegin 1st peg retrieval");
+    bluetooth.printf("\nBegin 1st peg retrieval");
+    
+    drive.move(13.0, 0.0);
+    scanner.setLocalizeRightFlag(0);
+pc.printf("\n    LocalizeRight on");
+bluetooth.printf("\n    LocalizeRight on");
+    scanner.setLocalizeLeftFlag(0);
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    pc.printf("\nMoved from 0 to 1");
+    bluetooth.printf("\nMoved from 0 to 1");
+    
+    scanner.setLocalizeRightFlag(0);
+pc.printf("\n    LocalizeRight off");
+bluetooth.printf("\n    LocalizeRight off");
+    scanner.setLocalizeLeftFlag(0);
+    drive.move(0.0, (-90.0)*(3.14159 / 180.0));
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    pc.printf("\nMoved left 90 degrees");
+    bluetooth.printf("\nMoved left 90 degrees");
+    
+    drive.move(15.0, 0.0);
+    scanner.setLocalizeRightFlag(0);
+    scanner.setLocalizeLeftFlag(0);
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    pc.printf("\nMoved from 1 to 3");
+    bluetooth.printf("\nMoved from 1 to 3");
+    
+    scanner.setLocalizeRightFlag(0);
+    scanner.setLocalizeLeftFlag(0);
+    drive.move(0.0, (90.0)*(3.14159 / 180.0));
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    pc.printf("\nMoved right 90 degrees");
+    bluetooth.printf("\nMoved right 90 degrees");
+    
+    drive.move(6.0, 0.0);
+    scanner.setLocalizeRightFlag(0);
+    scanner.setLocalizeLeftFlag(0);
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    pc.printf("\nMoved from 3 to 15");
+    bluetooth.printf("\nMoved from 3 to 15");
+    
+    drive.move(60.0, 0.0);
+    scanner.setLocalizeRightFlag(1);
+pc.printf("\n    LocalizeRight on");
+    scanner.setLocalizeLeftFlag(0);
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    pc.printf("\nMoved from 15 to 7");
+    bluetooth.printf("\nMoved from 15 to 7");
+    
+    pc.printf("\nExercise Complete");
+    bluetooth.printf("\nExercise Complete");
+}
\ No newline at end of file