Diff: mainV12.cpp

Revision:

17:ec52258b9472

Parent:

16:570134040cdf

--- a/mainV12.cpp	Mon Jun 05 07:57:07 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,115 +0,0 @@
-#include "mbed.h"
-#include "Brobot.h"
-//#include "PID_Control.h"
-#include "SpeedControl.h"
-
-//communication
-Serial pc(USBTX, USBRX);
-Serial cam(PA_9, PA_10);  // we are only using PA_9 to recieve
-
-//Perophery for distance sensors
-AnalogIn distance(PB_1);
-DigitalIn button(USER_BUTTON);
-DigitalOut enable(PC_1);  // sensors
-DigitalOut bit0(PH_1);
-DigitalOut bit1(PC_2);
-DigitalOut bit2(PC_3);
-DigitalOut led(LED1);
-Pixy pixy(cam);
-
-
-//indicator leds arround robot
-DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
-
-//motor stuff
-DigitalOut enableMotorDriver(PB_2);
-PwmOut pwmL(PA_8);
-PwmOut pwmR(PA_9);
-UniServ servo(PC_4); // Digital Out Pin
-
-EncoderCounter counterLeft(PB_6, PB_7);
-EncoderCounter counterRight(PA_6, PC_7);
-
-SpeedControl speedctrl(&pwmL, &pwmR, &counterLeft, &counterRight);
-
-Brobot stingray(speedctrl, distance, enable, bit0, bit1, bit2, leds, pixy, servo);  // passing in variables connected to PIN
-// should see LEDshow once constructed
-
-int main()
-{
-    enableMotorDriver = 0;  //safety precaution
-    enable = 1 ;            //for debugging and output
-
-    enum robot_states {idle=0, search, approach, goHome, atHome};   //so that we can reference states by name vs number
-    int robot_state=idle;    // define and start in idle state
-    int count = 0 ;
-
-    while(1) {
-        wait( 0.02f ); //required to allow time for ticker
-
-        stingray.startLeds();
-
-        switch(robot_state) {
-            case idle:
-                if(button == 0) {       // 0 is pressed
-                    enableMotorDriver =1;
-                    robot_state=search;         // next state
-                    //printf("leaving the idle state \n\r");
-                    //wait(0.1);
-                }
-                break;
-
-            case search:
-                /*
-                while( !stingray.foundGreenBrick() ) {
-                    stingray.avoidObstacleAndMove(20);  // in rpm
-                }
-                */
-
-                stingray.avoidObstacleAndMove(20);  // in rpm
-                //robot_state=approach;
-                break;
-
-            case approach:
-            
-                stingray.stop();
-
-                //stingray.approachBrick();
-                //stingray.closeGrip();
-                // find way to determine if lego is successfully grabbed before switching states
-                // robot_state=goHome;
-
-                break;
-
-            case goHome:
-
-                //printf("In the GoHome state \n\r");
-
-                if(stingray.foundHome() == false && count<=5) { // tune 5 so that robot rotates ~360 degrees
-                    stingray.rotate(10);
-                    count++;
-                }
-                while( stingray.foundHome() == false && count>5 ) {
-                    stingray.avoidObstacleAndMove(20);  // search until home is found
-                }
-                // if this point is reached, we have foundHome() == true
-                stingray.approachHome();
-
-                robot_state = atHome;
-                count =0;
-
-                break;
-
-            case atHome:
-
-                stingray.openGrip();
-                wait(0.5);  // may not be necessary but allows grip time
-                stingray.back(); // goes at 15rpms
-                wait(1);
-                stingray.rotate(10);  // enter rotational velocity in rpm between +-25
-                wait(0.5);  // tune so that rotation is about 180deg
-                robot_state=search;  // restart at searching state
-
-        }
-    }
-}