Mechatronics Robotics / Mbed 2 deprecated BrobotV1

These are the core files for the Robot at Team conception.

Dependencies:   mbed UniServ

mainV12.cpp@15:4efc66de795a, 2017-06-05 (annotated)

Committer:
obrie829
Date:
Mon Jun 05 07:56:08 2017 +0000
Revision:
15:4efc66de795a
blah

Who changed what in which revision?

UserRevisionLine numberNew contents of line
obrie829 15:4efc66de795a 1 #include "mbed.h"
obrie829 15:4efc66de795a 2 #include "Brobot.h"
obrie829 15:4efc66de795a 3 //#include "PID_Control.h"
obrie829 15:4efc66de795a 4 #include "SpeedControl.h"
obrie829 15:4efc66de795a 5
obrie829 15:4efc66de795a 6 //communication
obrie829 15:4efc66de795a 7 Serial pc(USBTX, USBRX);
obrie829 15:4efc66de795a 8 Serial cam(PA_9, PA_10); // we are only using PA_9 to recieve
obrie829 15:4efc66de795a 9
obrie829 15:4efc66de795a 10 //Perophery for distance sensors
obrie829 15:4efc66de795a 11 AnalogIn distance(PB_1);
obrie829 15:4efc66de795a 12 DigitalIn button(USER_BUTTON);
obrie829 15:4efc66de795a 13 DigitalOut enable(PC_1); // sensors
obrie829 15:4efc66de795a 14 DigitalOut bit0(PH_1);
obrie829 15:4efc66de795a 15 DigitalOut bit1(PC_2);
obrie829 15:4efc66de795a 16 DigitalOut bit2(PC_3);
obrie829 15:4efc66de795a 17 DigitalOut led(LED1);
obrie829 15:4efc66de795a 18 Pixy pixy(cam);
obrie829 15:4efc66de795a 19
obrie829 15:4efc66de795a 20
obrie829 15:4efc66de795a 21 //indicator leds arround robot
obrie829 15:4efc66de795a 22 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
obrie829 15:4efc66de795a 23
obrie829 15:4efc66de795a 24 //motor stuff
obrie829 15:4efc66de795a 25 DigitalOut enableMotorDriver(PB_2);
obrie829 15:4efc66de795a 26 PwmOut pwmL(PA_8);
obrie829 15:4efc66de795a 27 PwmOut pwmR(PA_9);
obrie829 15:4efc66de795a 28 UniServ servo(PC_4); // Digital Out Pin
obrie829 15:4efc66de795a 29
obrie829 15:4efc66de795a 30 EncoderCounter counterLeft(PB_6, PB_7);
obrie829 15:4efc66de795a 31 EncoderCounter counterRight(PA_6, PC_7);
obrie829 15:4efc66de795a 32
obrie829 15:4efc66de795a 33 SpeedControl speedctrl(&pwmL, &pwmR, &counterLeft, &counterRight);
obrie829 15:4efc66de795a 34
obrie829 15:4efc66de795a 35 Brobot stingray(speedctrl, distance, enable, bit0, bit1, bit2, leds, pixy, servo); // passing in variables connected to PIN
obrie829 15:4efc66de795a 36 // should see LEDshow once constructed
obrie829 15:4efc66de795a 37
obrie829 15:4efc66de795a 38 int main()
obrie829 15:4efc66de795a 39 {
obrie829 15:4efc66de795a 40 enableMotorDriver = 0; //safety precaution
obrie829 15:4efc66de795a 41 enable = 1 ; //for debugging and output
obrie829 15:4efc66de795a 42
obrie829 15:4efc66de795a 43 enum robot_states {idle=0, search, approach, goHome, atHome}; //so that we can reference states by name vs number
obrie829 15:4efc66de795a 44 int robot_state=idle; // define and start in idle state
obrie829 15:4efc66de795a 45 int count = 0 ;
obrie829 15:4efc66de795a 46
obrie829 15:4efc66de795a 47 while(1) {
obrie829 15:4efc66de795a 48 wait( 0.02f ); //required to allow time for ticker
obrie829 15:4efc66de795a 49
obrie829 15:4efc66de795a 50 stingray.startLeds();
obrie829 15:4efc66de795a 51
obrie829 15:4efc66de795a 52 switch(robot_state) {
obrie829 15:4efc66de795a 53 case idle:
obrie829 15:4efc66de795a 54 if(button == 0) { // 0 is pressed
obrie829 15:4efc66de795a 55 enableMotorDriver =1;
obrie829 15:4efc66de795a 56 robot_state=search; // next state
obrie829 15:4efc66de795a 57 //printf("leaving the idle state \n\r");
obrie829 15:4efc66de795a 58 //wait(0.1);
obrie829 15:4efc66de795a 59 }
obrie829 15:4efc66de795a 60 break;
obrie829 15:4efc66de795a 61
obrie829 15:4efc66de795a 62 case search:
obrie829 15:4efc66de795a 63 /*
obrie829 15:4efc66de795a 64 while( !stingray.foundGreenBrick() ) {
obrie829 15:4efc66de795a 65 stingray.avoidObstacleAndMove(20); // in rpm
obrie829 15:4efc66de795a 66 }
obrie829 15:4efc66de795a 67 */
obrie829 15:4efc66de795a 68
obrie829 15:4efc66de795a 69 stingray.avoidObstacleAndMove(20); // in rpm
obrie829 15:4efc66de795a 70 //robot_state=approach;
obrie829 15:4efc66de795a 71 break;
obrie829 15:4efc66de795a 72
obrie829 15:4efc66de795a 73 case approach:
obrie829 15:4efc66de795a 74
obrie829 15:4efc66de795a 75 stingray.stop();
obrie829 15:4efc66de795a 76
obrie829 15:4efc66de795a 77 //stingray.approachBrick();
obrie829 15:4efc66de795a 78 //stingray.closeGrip();
obrie829 15:4efc66de795a 79 // find way to determine if lego is successfully grabbed before switching states
obrie829 15:4efc66de795a 80 // robot_state=goHome;
obrie829 15:4efc66de795a 81
obrie829 15:4efc66de795a 82 break;
obrie829 15:4efc66de795a 83
obrie829 15:4efc66de795a 84 case goHome:
obrie829 15:4efc66de795a 85
obrie829 15:4efc66de795a 86 //printf("In the GoHome state \n\r");
obrie829 15:4efc66de795a 87
obrie829 15:4efc66de795a 88 if(stingray.foundHome() == false && count<=5) { // tune 5 so that robot rotates ~360 degrees
obrie829 15:4efc66de795a 89 stingray.rotate(10);
obrie829 15:4efc66de795a 90 count++;
obrie829 15:4efc66de795a 91 }
obrie829 15:4efc66de795a 92 while( stingray.foundHome() == false && count>5 ) {
obrie829 15:4efc66de795a 93 stingray.avoidObstacleAndMove(20); // search until home is found
obrie829 15:4efc66de795a 94 }
obrie829 15:4efc66de795a 95 // if this point is reached, we have foundHome() == true
obrie829 15:4efc66de795a 96 stingray.approachHome();
obrie829 15:4efc66de795a 97
obrie829 15:4efc66de795a 98 robot_state = atHome;
obrie829 15:4efc66de795a 99 count =0;
obrie829 15:4efc66de795a 100
obrie829 15:4efc66de795a 101 break;
obrie829 15:4efc66de795a 102
obrie829 15:4efc66de795a 103 case atHome:
obrie829 15:4efc66de795a 104
obrie829 15:4efc66de795a 105 stingray.openGrip();
obrie829 15:4efc66de795a 106 wait(0.5); // may not be necessary but allows grip time
obrie829 15:4efc66de795a 107 stingray.back(); // goes at 15rpms
obrie829 15:4efc66de795a 108 wait(1);
obrie829 15:4efc66de795a 109 stingray.rotate(10); // enter rotational velocity in rpm between +-25
obrie829 15:4efc66de795a 110 wait(0.5); // tune so that rotation is about 180deg
obrie829 15:4efc66de795a 111 robot_state=search; // restart at searching state
obrie829 15:4efc66de795a 112
obrie829 15:4efc66de795a 113 }
obrie829 15:4efc66de795a 114 }
obrie829 15:4efc66de795a 115 }