Mechatronics Robotics
/
BrobotV1
These are the core files for the Robot at Team conception.
Diff: mainV12.cpp
- Revision:
- 17:ec52258b9472
- Parent:
- 16:570134040cdf
diff -r 570134040cdf -r ec52258b9472 mainV12.cpp --- 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 - - } - } -}