Diff: mainV12.cpp

Revision:

15:4efc66de795a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mainV12.cpp	Mon Jun 05 07:56:08 2017 +0000
@@ -0,0 +1,115 @@
+#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
+
+        }
+    }
+}