stepper test
Dependencies: LongRangeSensor R5_StepperDrive mbed scanner
main.cpp
- Committer:
- j_j205
- Date:
- 2016-04-01
- Revision:
- 0:76bca1f580a0
File content as of revision 0:76bca1f580a0:
#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"); }