Mechatronics Robotics / Mbed 2 deprecated BrobotV1

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

Dependencies:   mbed UniServ

Brobot.cpp@17:ec52258b9472, 2017-06-07 (annotated)

Committer:
obrie829
Date:
Wed Jun 07 11:35:59 2017 +0000
Revision:
17:ec52258b9472
Parent:
15:4efc66de795a 
v18

Who changed what in which revision?

UserRevisionLine numberNew contents of line
obrie829 0:eba74e7a229b 1 /*
obrie829 0:eba74e7a229b 2 * BROBOT.cpp
obrie829 0:eba74e7a229b 3 *
obrie829 0:eba74e7a229b 4 */
obrie829 0:eba74e7a229b 5
obrie829 0:eba74e7a229b 6 #include <cmath>
obrie829 0:eba74e7a229b 7 #include "Brobot.h"
obrie829 15:4efc66de795a 8
obrie829 15:4efc66de795a 9 Brobot::Brobot(SpeedControl& speedctrl_, AnalogIn& distance_, DigitalOut& enable_, DigitalOut& bit0_, DigitalOut& bit1_, DigitalOut& bit2_, DigitalOut* led_ptr, Pixy& pixy_, UniServ& servo_) :
obrie829 15:4efc66de795a 10 speedctrl(speedctrl_), distance(distance_), enable(enable_), bit0(bit0_), bit1(bit1_), bit2(bit2_), leds(led_ptr), pixy(pixy_), servo(servo_) // assigning parameters to class variables
obrie829 15:4efc66de795a 11 {
obrie829 15:4efc66de795a 12 //initialize distance sensors
obrie829 15:4efc66de795a 13 for( int ii = 0; ii<6; ++ii)
obrie829 15:4efc66de795a 14 sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
obrie829 15:4efc66de795a 15
obrie829 15:4efc66de795a 16 //defining the sensors for
obrie829 15:4efc66de795a 17 sensor_front.init( &distance, &bit0, &bit1, &bit2, 0); // & give the address of the object
obrie829 15:4efc66de795a 18 sensor_left.init( &distance, &bit0, &bit1, &bit2, 5);
obrie829 15:4efc66de795a 19 sensor_right.init( &distance, &bit0, &bit1, &bit2, 1);
obrie829 15:4efc66de795a 20
obrie829 15:4efc66de795a 21 // kp ki kd min max
obrie829 15:4efc66de795a 22 //pid.setPIDValues( 0.025f, 0.00005f, 0.001f, 0.5f, -0.5f, 1000);
obrie829 17:ec52258b9472 23 pid.setPIDValues( 0.4f, 0.001f, 0.001f, 0.5f, -0.5f, 1000);
obrie829 17:ec52258b9472 24 pixypid.setPIDValues( 0.005f, 0.0f, 0.000f, 25.0f, -25.0f, 1000);
obrie829 17:ec52258b9472 25 //pixypidS.setPIDValues( 0.01f, 0.0f, 0.0f, 20.0f,-20.0f, 1000);
obrie829 17:ec52258b9472 26 //e = 0 ;
obrie829 0:eba74e7a229b 27
obrie829 15:4efc66de795a 28 }
obrie829 15:4efc66de795a 29
obrie829 15:4efc66de795a 30 void Brobot::avoidObstacleAndMove(int vtrans)
obrie829 15:4efc66de795a 31 {
obrie829 15:4efc66de795a 32 float vrot = 0; // rpm
obrie829 15:4efc66de795a 33
obrie829 15:4efc66de795a 34 if(sensor_left.read() <0.3f || sensor_right.read() <0.3f) { // to avoid obstacles
obrie829 15:4efc66de795a 35
obrie829 15:4efc66de795a 36 e = sensor_left.read() - sensor_right.read();
obrie829 15:4efc66de795a 37 float diff = pid.calc( e, 0.05f ); // error and period are arguments
obrie829 15:4efc66de795a 38
obrie829 15:4efc66de795a 39 vrot = diff*50; //turn
obrie829 15:4efc66de795a 40
obrie829 17:ec52258b9472 41 if( vrot<= -10)
obrie829 17:ec52258b9472 42 vrot=-10;
obrie829 17:ec52258b9472 43 else if (vrot > 10)
obrie829 17:ec52258b9472 44 vrot=10;
obrie829 15:4efc66de795a 45 }
obrie829 0:eba74e7a229b 46
obrie829 15:4efc66de795a 47 speedctrl.setDesiredSpeed( vtrans - vrot, -vtrans - vrot );
obrie829 15:4efc66de795a 48
obrie829 15:4efc66de795a 49 if(sensor_front.read() <=0.25f) { // when approaching normal to wall
obrie829 15:4efc66de795a 50 speedctrl.setDesiredSpeed( 0, 0 );
obrie829 15:4efc66de795a 51
obrie829 15:4efc66de795a 52 int direction=rand()%2 ; // so there is variablility in the robots path
obrie829 15:4efc66de795a 53 if(direction==0) {
obrie829 17:ec52258b9472 54 speedctrl.setDesiredSpeed(5, 5 );
obrie829 17:ec52258b9472 55 wait(0.5);
obrie829 15:4efc66de795a 56 } else if (direction==1) {
obrie829 17:ec52258b9472 57 speedctrl.setDesiredSpeed( -5, -5 );
obrie829 17:ec52258b9472 58 wait(0.5);
obrie829 15:4efc66de795a 59 }
obrie829 15:4efc66de795a 60 }
obrie829 15:4efc66de795a 61 }
obrie829 15:4efc66de795a 62
obrie829 15:4efc66de795a 63 void Brobot::ledDistance()
obrie829 15:4efc66de795a 64 {
obrie829 15:4efc66de795a 65 for( int ii = 0; ii<6; ++ii)
obrie829 15:4efc66de795a 66 sensors[ii]< 0.1f ? leds[ii].write(1) : leds[ii].write(0); // an if statement in one line
obrie829 17:ec52258b9472 67 //printf("the L and R sensor values are %f and %f \n\r", sensor_left, sensor_right);
obrie829 15:4efc66de795a 68 }
obrie829 15:4efc66de795a 69
obrie829 15:4efc66de795a 70 void Brobot::ledShow()
obrie829 15:4efc66de795a 71 {
obrie829 15:4efc66de795a 72 static int timer = 0; // static is only the first time/loop
obrie829 15:4efc66de795a 73 // for( int ii = 0; ii<6; ++ii)
obrie829 15:4efc66de795a 74 // leds[ii] = !leds[ii];
obrie829 15:4efc66de795a 75
obrie829 15:4efc66de795a 76 //quit ticker and start led distance show
obrie829 15:4efc66de795a 77 if( ++timer > 10) {
obrie829 15:4efc66de795a 78 t1.detach();
obrie829 17:ec52258b9472 79 t1.attach( callback(this, &Brobot::ledDistance), 0.05f );
obrie829 15:4efc66de795a 80 }
obrie829 0:eba74e7a229b 81 }
obrie829 0:eba74e7a229b 82
obrie829 15:4efc66de795a 83 bool Brobot::foundGreenBrick()
obrie829 15:4efc66de795a 84 {
obrie829 15:4efc66de795a 85 if( pixy.objectDetected() ) {
obrie829 17:ec52258b9472 86 if( pixy.getSignature() == 1) { // 1 is the green brick
obrie829 15:4efc66de795a 87 return true;
obrie829 15:4efc66de795a 88 }
obrie829 15:4efc66de795a 89 }
obrie829 15:4efc66de795a 90 return false;
obrie829 15:4efc66de795a 91 }
obrie829 15:4efc66de795a 92
obrie829 15:4efc66de795a 93 bool Brobot::foundHome()
obrie829 0:eba74e7a229b 94 {
obrie829 15:4efc66de795a 95 if( pixy.objectDetected() ) {
obrie829 17:ec52258b9472 96 if( pixy.getSignature() == 2) { // 2 is the red home
obrie829 15:4efc66de795a 97 return true;
obrie829 15:4efc66de795a 98 }
obrie829 0:eba74e7a229b 99 }
obrie829 15:4efc66de795a 100 return false;
obrie829 15:4efc66de795a 101 }
obrie829 0:eba74e7a229b 102
obrie829 17:ec52258b9472 103 int Brobot::rotateAndApproach()
obrie829 15:4efc66de795a 104 {
obrie829 17:ec52258b9472 105 static float objectDetected = 1.0f; // assumes we can already detect object
obrie829 17:ec52258b9472 106
obrie829 17:ec52258b9472 107 objectDetected = 0.95f * objectDetected + 0.05f * pixy.objectDetected();// filtering for flickering pixy
obrie829 17:ec52258b9472 108
obrie829 17:ec52258b9472 109 if( pixy.getSignature() == 1 && objectDetected > 0.2f) {
obrie829 17:ec52258b9472 110
obrie829 17:ec52258b9472 111 leds[3] = !leds[3] ;
obrie829 17:ec52258b9472 112
obrie829 17:ec52258b9472 113 float pixyX = pixy.getX();
obrie829 17:ec52258b9472 114 float pixyY = pixy.getY();
obrie829 17:ec52258b9472 115 float error = 160-pixyX;
obrie829 17:ec52258b9472 116 float vrot = error * 0.25f ;
obrie829 17:ec52258b9472 117 float vtrans = (200.0f - pixyY) * 0.25f;
obrie829 17:ec52258b9472 118
obrie829 17:ec52258b9472 119 if( vrot > 5) vrot = 5;
obrie829 17:ec52258b9472 120 if( vrot < -5) vrot = -5;
obrie829 17:ec52258b9472 121 if( vtrans > 5) vtrans = 5;
obrie829 17:ec52258b9472 122 if( vtrans < -5) vtrans = -5;
obrie829 17:ec52258b9472 123
obrie829 17:ec52258b9472 124 //speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
obrie829 17:ec52258b9472 125 speedctrl.setDesiredSpeed( -vrot , -vrot ) ;
obrie829 17:ec52258b9472 126
obrie829 17:ec52258b9472 127 if ((error < 15)&&(error>-15)) {
obrie829 17:ec52258b9472 128 speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
obrie829 17:ec52258b9472 129 }
obrie829 17:ec52258b9472 130
obrie829 17:ec52258b9472 131 if( pixyY >= 180) { // assumes centered on brick
obrie829 17:ec52258b9472 132 speedctrl.setDesiredSpeed(8.0f, -8.0f); // blindly drive forward
obrie829 17:ec52258b9472 133 wait(0.5);
obrie829 17:ec52258b9472 134 Brobot::closeGrip();
obrie829 17:ec52258b9472 135 return 1; // need to transfer to GoHome state
obrie829 17:ec52258b9472 136 }
obrie829 17:ec52258b9472 137 return 3; // continue the approach function
obrie829 17:ec52258b9472 138 } else return 2; // return to search state, no object is detected
obrie829 0:eba74e7a229b 139 }
obrie829 0:eba74e7a229b 140
obrie829 15:4efc66de795a 141 bool Brobot::approachHome()
obrie829 8:351b0b7b05b2 142 {
obrie829 17:ec52258b9472 143 static float objectDetected = 1.0f; // assumes we can already detect object
obrie829 15:4efc66de795a 144
obrie829 17:ec52258b9472 145 objectDetected = 0.98f * objectDetected + 0.02f * pixy.objectDetected();// filtering for flickering pixy
obrie829 17:ec52258b9472 146 leds[4]=0;
obrie829 17:ec52258b9472 147 if(objectDetected>0.1f) leds[4]=1;
obrie829 17:ec52258b9472 148
obrie829 17:ec52258b9472 149 if( (pixy.getSignature() == 2) && (objectDetected > 0.1f)) {
obrie829 17:ec52258b9472 150
obrie829 17:ec52258b9472 151 float pixyX = pixy.getX();
obrie829 17:ec52258b9472 152 float pixyY = pixy.getY();
obrie829 17:ec52258b9472 153 float error = 160-pixyX;
obrie829 17:ec52258b9472 154 float vrot = error * 0.25f ;
obrie829 17:ec52258b9472 155 float vtrans = (200.0f - pixyY) * 0.25f;
obrie829 17:ec52258b9472 156
obrie829 15:4efc66de795a 157
obrie829 17:ec52258b9472 158 if( pixyY >= 50) return true; // need to transfer to atHome state
obrie829 17:ec52258b9472 159 if( vrot > 4) vrot = 4;
obrie829 17:ec52258b9472 160 if( vrot < -4) vrot = -4;
obrie829 17:ec52258b9472 161 if( vtrans > 4) vtrans = 4;
obrie829 17:ec52258b9472 162 if( vtrans < -4) vtrans = -4;
obrie829 15:4efc66de795a 163
obrie829 17:ec52258b9472 164 //speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
obrie829 17:ec52258b9472 165 speedctrl.setDesiredSpeed( -vrot , -vrot ) ;
obrie829 17:ec52258b9472 166
obrie829 17:ec52258b9472 167 if ((error <= 25) && (error>= -25)) {
obrie829 17:ec52258b9472 168 speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
obrie829 15:4efc66de795a 169 }
obrie829 17:ec52258b9472 170
obrie829 17:ec52258b9472 171 return false;
obrie829 17:ec52258b9472 172 } else return false; // return to goHome case, no object is detected
obrie829 8:351b0b7b05b2 173 }
obrie829 15:4efc66de795a 174
obrie829 15:4efc66de795a 175 void Brobot::closeGrip()
obrie829 15:4efc66de795a 176 {
obrie829 17:ec52258b9472 177 int close=1400;
obrie829 15:4efc66de795a 178 servo.write_us(close);
obrie829 17:ec52258b9472 179 wait(0.5);
obrie829 15:4efc66de795a 180 }
obrie829 15:4efc66de795a 181
obrie829 15:4efc66de795a 182 void Brobot::openGrip()
obrie829 15:4efc66de795a 183 {
obrie829 17:ec52258b9472 184 int open=1900;
obrie829 15:4efc66de795a 185 servo.write_us(open);
obrie829 17:ec52258b9472 186 wait(0.5);
obrie829 15:4efc66de795a 187 }
obrie829 15:4efc66de795a 188
obrie829 15:4efc66de795a 189
obrie829 15:4efc66de795a 190 void Brobot::rotate(int phi)
obrie829 15:4efc66de795a 191 {
obrie829 15:4efc66de795a 192 if(phi>25|| phi<-25) {
obrie829 15:4efc66de795a 193 phi=10;
obrie829 15:4efc66de795a 194 }
obrie829 15:4efc66de795a 195 speedctrl.setDesiredSpeed(phi, phi);
obrie829 15:4efc66de795a 196 }
obrie829 8:351b0b7b05b2 197
obrie829 0:eba74e7a229b 198 void Brobot::forward()
obrie829 0:eba74e7a229b 199 {
obrie829 15:4efc66de795a 200 speedctrl.setDesiredSpeed(20, -20); // rpms
obrie829 0:eba74e7a229b 201 }
obrie829 0:eba74e7a229b 202
obrie829 0:eba74e7a229b 203 void Brobot::back()
obrie829 0:eba74e7a229b 204 {
obrie829 15:4efc66de795a 205 speedctrl.setDesiredSpeed(-15, 15); //rpms
obrie829 0:eba74e7a229b 206 }
obrie829 0:eba74e7a229b 207
obrie829 0:eba74e7a229b 208 void Brobot::stop()
obrie829 0:eba74e7a229b 209 {
obrie829 15:4efc66de795a 210 speedctrl.setDesiredSpeed(0.0f, 0.0f);
obrie829 0:eba74e7a229b 211 }