Mechatronics Robotics / Mbed 2 deprecated BrobotV1

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

Dependencies:   mbed UniServ

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?

UserRevisionLine numberNew 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 }