Mechatronics Robotics
/
BrobotV1
These are the core files for the Robot at Team conception.
mainV12.cpp@16:570134040cdf, 2017-06-05 (annotated)
- Committer:
- obrie829
- Date:
- Mon Jun 05 07:57:07 2017 +0000
- Revision:
- 16:570134040cdf
- Parent:
- 15:4efc66de795a
v12
Who changed what in which revision?
User | Revision | Line number | New 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 | } |