Diff: mainV18.cpp

Revision:

17:ec52258b9472

diff -r 570134040cdf -r ec52258b9472 mainV18.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mainV18.cpp	Wed Jun 07 11:35:59 2017 +0000
@@ -0,0 +1,111 @@
+#include "mbed.h"
+#include "Brobot.h"
+#include "SpeedControl.h"
+
+//communication
+Serial pc(USBTX, USBRX);
+Serial cam(PB_10, PC_5);  // 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
+
+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 ;
+    int timer = 0;
+
+    while(1) {
+        wait( 0.01f ); //required to allow time for ticker
+
+        if( ++timer%100==0)
+            led=!led;
+
+        switch(robot_state) {
+            case idle:
+                if(button == 0) {       // 0 is pressed
+                    enableMotorDriver = 1;
+                    robot_state=search;         // next state
+                }
+                break;
+
+            case search:
+
+                stingray.avoidObstacleAndMove(12);  // in rpm
+                stingray.openGrip();  //just trying to get lucky
+
+                if(stingray.foundGreenBrick()) {
+                    stingray.stop();
+                    leds[3] = 1;
+                    robot_state = approach;
+                }
+                break;
+
+            case approach:
+
+                int rotateAndApproachState = stingray.rotateAndApproach();
+                if(rotateAndApproachState== 1) robot_state = goHome ; // gripper has block
+                else if(rotateAndApproachState == 2) robot_state = search ; // block never made it to gripper\
+                count = 0;
+
+                break; // continue approach
+
+            case goHome:
+
+                if(stingray.foundHome() == false && count <= 750) { // tune 5 so that robot rotates ~360 degrees
+                    stingray.rotate(5);
+                    count++;
+                }
+                if( stingray.foundHome() == false && count > 750 ) {
+                    stingray.avoidObstacleAndMove(10);  // search until home is found
+                }
+                static bool foundHomeFlag=false;
+                // if this point is reached, we have foundHome() == true
+                if( stingray.foundHome() || foundHomeFlag ) {
+                    foundHomeFlag=true;
+                    if(stingray.approachHome())
+                        robot_state = atHome ;
+                }
+                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
+
+        }
+    }
+}