mainV18.cpp

00001 #include "mbed.h"
00002 #include "Brobot.h"
00003 #include "SpeedControl.h"
00004 
00005 //communication
00006 Serial pc(USBTX, USBRX);
00007 Serial cam(PB_10, PC_5);  // we are only using PA_9 to recieve
00008 
00009 //Perophery for distance sensors
00010 AnalogIn distance(PB_1);
00011 DigitalIn button(USER_BUTTON);
00012 DigitalOut enable(PC_1);  // sensors
00013 DigitalOut bit0(PH_1);
00014 DigitalOut bit1(PC_2);
00015 DigitalOut bit2(PC_3);
00016 DigitalOut led(LED1);
00017 Pixy pixy(cam);
00018 
00019 
00020 //indicator leds arround robot
00021 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
00022 
00023 //motor stuff
00024 DigitalOut enableMotorDriver(PB_2);
00025 PwmOut pwmL(PA_8);
00026 PwmOut pwmR(PA_9);
00027 UniServ servo(PC_4); // Digital Out Pin
00028 
00029 EncoderCounter counterLeft(PB_6, PB_7);
00030 EncoderCounter counterRight(PA_6, PC_7);
00031 
00032 SpeedControl speedctrl(&pwmL, &pwmR, &counterLeft, &counterRight);
00033 
00034 Brobot stingray(speedctrl, distance, enable, bit0, bit1, bit2, leds, pixy, servo);  // passing in variables connected to PIN
00035 
00036 int main()
00037 {
00038     enableMotorDriver = 0;  //safety precaution
00039     enable = 1 ;            //for debugging and output
00040 
00041     enum robot_states {idle=0, search, approach, goHome, atHome};   //so that we can reference states by name vs number
00042     int robot_state  = idle;    // define and start in idle state
00043     int count = 0 ;
00044     int timer = 0;
00045 
00046     while(1) {
00047         wait( 0.01f ); //required to allow time for ticker
00048 
00049         if( ++timer%100==0)
00050             led=!led;
00051 
00052         switch(robot_state) {
00053             case idle:
00054                 if(button == 0) {       // 0 is pressed
00055                     enableMotorDriver = 1;
00056                     robot_state=search;         // next state
00057                 }
00058                 break;
00059 
00060             case search:
00061 
00062                 stingray.avoidObstacleAndMove(12);  // in rpm
00063                 stingray.openGrip();  //just trying to get lucky
00064 
00065                 if(stingray.foundGreenBrick()) {
00066                     stingray.stop();
00067                     leds[3] = 1;
00068                     robot_state = approach;
00069                 }
00070                 break;
00071 
00072             case approach:
00073 
00074                 int rotateAndApproachState = stingray.rotateAndApproach();
00075                 if(rotateAndApproachState== 1) robot_state = goHome ; // gripper has block
00076                 else if(rotateAndApproachState == 2) robot_state = search ; // block never made it to gripper\
00077                 count = 0;
00078 
00079                 break; // continue approach
00080 
00081             case goHome:
00082 
00083                 if(stingray.foundHome() == false && count <= 750) { // tune 5 so that robot rotates ~360 degrees
00084                     stingray.rotate(5);
00085                     count++;
00086                 }
00087                 if( stingray.foundHome() == false && count > 750 ) {
00088                     stingray.avoidObstacleAndMove(10);  // search until home is found
00089                 }
00090                 static bool foundHomeFlag=false;
00091                 // if this point is reached, we have foundHome() == true
00092                 if( stingray.foundHome() || foundHomeFlag ) {
00093                     foundHomeFlag=true;
00094                     if(stingray.approachHome())
00095                         robot_state = atHome ;
00096                 }
00097                 break;
00098 
00099             case atHome:
00100 
00101                 stingray.openGrip();
00102                 wait(0.5);  // may not be necessary but allows grip time
00103                 stingray.back(); // goes at 15rpms
00104                 wait(1);
00105                 stingray.rotate(10);  // enter rotational velocity in rpm between +-25
00106                 wait(0.5);  // tune so that rotation is about 180deg
00107                 robot_state=search;  // restart at searching state
00108 
00109         }
00110     }
00111 }