Mechatronics Robotics
/
BrobotV1
These are the core files for the Robot at Team conception.
mainV18.cpp@17:ec52258b9472, 2017-06-07 (annotated)
- Committer:
- obrie829
- Date:
- Wed Jun 07 11:35:59 2017 +0000
- Revision:
- 17:ec52258b9472
v18
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
obrie829 | 17:ec52258b9472 | 1 | #include "mbed.h" |
obrie829 | 17:ec52258b9472 | 2 | #include "Brobot.h" |
obrie829 | 17:ec52258b9472 | 3 | #include "SpeedControl.h" |
obrie829 | 17:ec52258b9472 | 4 | |
obrie829 | 17:ec52258b9472 | 5 | //communication |
obrie829 | 17:ec52258b9472 | 6 | Serial pc(USBTX, USBRX); |
obrie829 | 17:ec52258b9472 | 7 | Serial cam(PB_10, PC_5); // we are only using PA_9 to recieve |
obrie829 | 17:ec52258b9472 | 8 | |
obrie829 | 17:ec52258b9472 | 9 | //Perophery for distance sensors |
obrie829 | 17:ec52258b9472 | 10 | AnalogIn distance(PB_1); |
obrie829 | 17:ec52258b9472 | 11 | DigitalIn button(USER_BUTTON); |
obrie829 | 17:ec52258b9472 | 12 | DigitalOut enable(PC_1); // sensors |
obrie829 | 17:ec52258b9472 | 13 | DigitalOut bit0(PH_1); |
obrie829 | 17:ec52258b9472 | 14 | DigitalOut bit1(PC_2); |
obrie829 | 17:ec52258b9472 | 15 | DigitalOut bit2(PC_3); |
obrie829 | 17:ec52258b9472 | 16 | DigitalOut led(LED1); |
obrie829 | 17:ec52258b9472 | 17 | Pixy pixy(cam); |
obrie829 | 17:ec52258b9472 | 18 | |
obrie829 | 17:ec52258b9472 | 19 | |
obrie829 | 17:ec52258b9472 | 20 | //indicator leds arround robot |
obrie829 | 17:ec52258b9472 | 21 | DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; |
obrie829 | 17:ec52258b9472 | 22 | |
obrie829 | 17:ec52258b9472 | 23 | //motor stuff |
obrie829 | 17:ec52258b9472 | 24 | DigitalOut enableMotorDriver(PB_2); |
obrie829 | 17:ec52258b9472 | 25 | PwmOut pwmL(PA_8); |
obrie829 | 17:ec52258b9472 | 26 | PwmOut pwmR(PA_9); |
obrie829 | 17:ec52258b9472 | 27 | UniServ servo(PC_4); // Digital Out Pin |
obrie829 | 17:ec52258b9472 | 28 | |
obrie829 | 17:ec52258b9472 | 29 | EncoderCounter counterLeft(PB_6, PB_7); |
obrie829 | 17:ec52258b9472 | 30 | EncoderCounter counterRight(PA_6, PC_7); |
obrie829 | 17:ec52258b9472 | 31 | |
obrie829 | 17:ec52258b9472 | 32 | SpeedControl speedctrl(&pwmL, &pwmR, &counterLeft, &counterRight); |
obrie829 | 17:ec52258b9472 | 33 | |
obrie829 | 17:ec52258b9472 | 34 | Brobot stingray(speedctrl, distance, enable, bit0, bit1, bit2, leds, pixy, servo); // passing in variables connected to PIN |
obrie829 | 17:ec52258b9472 | 35 | |
obrie829 | 17:ec52258b9472 | 36 | int main() |
obrie829 | 17:ec52258b9472 | 37 | { |
obrie829 | 17:ec52258b9472 | 38 | enableMotorDriver = 0; //safety precaution |
obrie829 | 17:ec52258b9472 | 39 | enable = 1 ; //for debugging and output |
obrie829 | 17:ec52258b9472 | 40 | |
obrie829 | 17:ec52258b9472 | 41 | enum robot_states {idle=0, search, approach, goHome, atHome}; //so that we can reference states by name vs number |
obrie829 | 17:ec52258b9472 | 42 | int robot_state = idle; // define and start in idle state |
obrie829 | 17:ec52258b9472 | 43 | int count = 0 ; |
obrie829 | 17:ec52258b9472 | 44 | int timer = 0; |
obrie829 | 17:ec52258b9472 | 45 | |
obrie829 | 17:ec52258b9472 | 46 | while(1) { |
obrie829 | 17:ec52258b9472 | 47 | wait( 0.01f ); //required to allow time for ticker |
obrie829 | 17:ec52258b9472 | 48 | |
obrie829 | 17:ec52258b9472 | 49 | if( ++timer%100==0) |
obrie829 | 17:ec52258b9472 | 50 | led=!led; |
obrie829 | 17:ec52258b9472 | 51 | |
obrie829 | 17:ec52258b9472 | 52 | switch(robot_state) { |
obrie829 | 17:ec52258b9472 | 53 | case idle: |
obrie829 | 17:ec52258b9472 | 54 | if(button == 0) { // 0 is pressed |
obrie829 | 17:ec52258b9472 | 55 | enableMotorDriver = 1; |
obrie829 | 17:ec52258b9472 | 56 | robot_state=search; // next state |
obrie829 | 17:ec52258b9472 | 57 | } |
obrie829 | 17:ec52258b9472 | 58 | break; |
obrie829 | 17:ec52258b9472 | 59 | |
obrie829 | 17:ec52258b9472 | 60 | case search: |
obrie829 | 17:ec52258b9472 | 61 | |
obrie829 | 17:ec52258b9472 | 62 | stingray.avoidObstacleAndMove(12); // in rpm |
obrie829 | 17:ec52258b9472 | 63 | stingray.openGrip(); //just trying to get lucky |
obrie829 | 17:ec52258b9472 | 64 | |
obrie829 | 17:ec52258b9472 | 65 | if(stingray.foundGreenBrick()) { |
obrie829 | 17:ec52258b9472 | 66 | stingray.stop(); |
obrie829 | 17:ec52258b9472 | 67 | leds[3] = 1; |
obrie829 | 17:ec52258b9472 | 68 | robot_state = approach; |
obrie829 | 17:ec52258b9472 | 69 | } |
obrie829 | 17:ec52258b9472 | 70 | break; |
obrie829 | 17:ec52258b9472 | 71 | |
obrie829 | 17:ec52258b9472 | 72 | case approach: |
obrie829 | 17:ec52258b9472 | 73 | |
obrie829 | 17:ec52258b9472 | 74 | int rotateAndApproachState = stingray.rotateAndApproach(); |
obrie829 | 17:ec52258b9472 | 75 | if(rotateAndApproachState== 1) robot_state = goHome ; // gripper has block |
obrie829 | 17:ec52258b9472 | 76 | else if(rotateAndApproachState == 2) robot_state = search ; // block never made it to gripper\ |
obrie829 | 17:ec52258b9472 | 77 | count = 0; |
obrie829 | 17:ec52258b9472 | 78 | |
obrie829 | 17:ec52258b9472 | 79 | break; // continue approach |
obrie829 | 17:ec52258b9472 | 80 | |
obrie829 | 17:ec52258b9472 | 81 | case goHome: |
obrie829 | 17:ec52258b9472 | 82 | |
obrie829 | 17:ec52258b9472 | 83 | if(stingray.foundHome() == false && count <= 750) { // tune 5 so that robot rotates ~360 degrees |
obrie829 | 17:ec52258b9472 | 84 | stingray.rotate(5); |
obrie829 | 17:ec52258b9472 | 85 | count++; |
obrie829 | 17:ec52258b9472 | 86 | } |
obrie829 | 17:ec52258b9472 | 87 | if( stingray.foundHome() == false && count > 750 ) { |
obrie829 | 17:ec52258b9472 | 88 | stingray.avoidObstacleAndMove(10); // search until home is found |
obrie829 | 17:ec52258b9472 | 89 | } |
obrie829 | 17:ec52258b9472 | 90 | static bool foundHomeFlag=false; |
obrie829 | 17:ec52258b9472 | 91 | // if this point is reached, we have foundHome() == true |
obrie829 | 17:ec52258b9472 | 92 | if( stingray.foundHome() || foundHomeFlag ) { |
obrie829 | 17:ec52258b9472 | 93 | foundHomeFlag=true; |
obrie829 | 17:ec52258b9472 | 94 | if(stingray.approachHome()) |
obrie829 | 17:ec52258b9472 | 95 | robot_state = atHome ; |
obrie829 | 17:ec52258b9472 | 96 | } |
obrie829 | 17:ec52258b9472 | 97 | break; |
obrie829 | 17:ec52258b9472 | 98 | |
obrie829 | 17:ec52258b9472 | 99 | case atHome: |
obrie829 | 17:ec52258b9472 | 100 | |
obrie829 | 17:ec52258b9472 | 101 | stingray.openGrip(); |
obrie829 | 17:ec52258b9472 | 102 | wait(0.5); // may not be necessary but allows grip time |
obrie829 | 17:ec52258b9472 | 103 | stingray.back(); // goes at 15rpms |
obrie829 | 17:ec52258b9472 | 104 | wait(1); |
obrie829 | 17:ec52258b9472 | 105 | stingray.rotate(10); // enter rotational velocity in rpm between +-25 |
obrie829 | 17:ec52258b9472 | 106 | wait(0.5); // tune so that rotation is about 180deg |
obrie829 | 17:ec52258b9472 | 107 | robot_state=search; // restart at searching state |
obrie829 | 17:ec52258b9472 | 108 | |
obrie829 | 17:ec52258b9472 | 109 | } |
obrie829 | 17:ec52258b9472 | 110 | } |
obrie829 | 17:ec52258b9472 | 111 | } |