![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
These are the core files for the Robot at Team conception.
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?
User | Revision | Line number | New 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 | } |