![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
These are the core files for the Robot at Team conception.
Brobot.cpp@15:4efc66de795a, 2017-06-05 (annotated)
- Committer:
- obrie829
- Date:
- Mon Jun 05 07:56:08 2017 +0000
- Revision:
- 15:4efc66de795a
- Parent:
- 8:351b0b7b05b2
- Child:
- 17:ec52258b9472
blah
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 | 15:4efc66de795a | 23 | pid.setPIDValues( 0.4f, 0.001f, 0.001f, 0.5f, -0.5f, 1000); |
obrie829 | 15:4efc66de795a | 24 | pixypid.setPIDValues( 0.0002f, 0.000f, 0.000f, 0.3f, -0.3f, 1000); |
obrie829 | 0:eba74e7a229b | 25 | |
obrie829 | 15:4efc66de795a | 26 | } |
obrie829 | 15:4efc66de795a | 27 | |
obrie829 | 15:4efc66de795a | 28 | void Brobot::avoidObstacleAndMove(int vtrans) |
obrie829 | 15:4efc66de795a | 29 | { |
obrie829 | 15:4efc66de795a | 30 | float vrot = 0; // rpm |
obrie829 | 15:4efc66de795a | 31 | static float e = 0.0f ; |
obrie829 | 15:4efc66de795a | 32 | |
obrie829 | 15:4efc66de795a | 33 | if(sensor_left.read() <0.3f || sensor_right.read() <0.3f) { // to avoid obstacles |
obrie829 | 15:4efc66de795a | 34 | |
obrie829 | 15:4efc66de795a | 35 | e = sensor_left.read() - sensor_right.read(); |
obrie829 | 15:4efc66de795a | 36 | float diff = pid.calc( e, 0.05f ); // error and period are arguments |
obrie829 | 15:4efc66de795a | 37 | |
obrie829 | 15:4efc66de795a | 38 | vrot = diff*50; //turn |
obrie829 | 15:4efc66de795a | 39 | |
obrie829 | 15:4efc66de795a | 40 | if( vrot<= -20) |
obrie829 | 15:4efc66de795a | 41 | vrot=-20; |
obrie829 | 15:4efc66de795a | 42 | else if (vrot > 20) |
obrie829 | 15:4efc66de795a | 43 | vrot=20; |
obrie829 | 15:4efc66de795a | 44 | } |
obrie829 | 0:eba74e7a229b | 45 | |
obrie829 | 15:4efc66de795a | 46 | speedctrl.setDesiredSpeed( vtrans - vrot, -vtrans - vrot ); |
obrie829 | 15:4efc66de795a | 47 | int delay = vtrans/100 ; |
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 | 15:4efc66de795a | 54 | //stingray.rotate(10); |
obrie829 | 15:4efc66de795a | 55 | speedctrl.setDesiredSpeed(15, 15 ); |
obrie829 | 15:4efc66de795a | 56 | wait(delay); |
obrie829 | 15:4efc66de795a | 57 | } else if (direction==1) { |
obrie829 | 15:4efc66de795a | 58 | //stingray.rotate(-10); |
obrie829 | 15:4efc66de795a | 59 | speedctrl.setDesiredSpeed( -15, -15 ); |
obrie829 | 15:4efc66de795a | 60 | wait(delay); |
obrie829 | 15:4efc66de795a | 61 | } |
obrie829 | 15:4efc66de795a | 62 | } |
obrie829 | 15:4efc66de795a | 63 | } |
obrie829 | 15:4efc66de795a | 64 | |
obrie829 | 15:4efc66de795a | 65 | void Brobot::startLeds() |
obrie829 | 8:351b0b7b05b2 | 66 | { |
obrie829 | 15:4efc66de795a | 67 | t1.attach( callback(this, &Brobot::ledShow), 0.05f ); |
obrie829 | 15:4efc66de795a | 68 | } |
obrie829 | 15:4efc66de795a | 69 | |
obrie829 | 15:4efc66de795a | 70 | void Brobot::ledDistance() |
obrie829 | 15:4efc66de795a | 71 | { |
obrie829 | 15:4efc66de795a | 72 | for( int ii = 0; ii<6; ++ii) |
obrie829 | 15:4efc66de795a | 73 | sensors[ii]< 0.1f ? leds[ii].write(1) : leds[ii].write(0); // an if statement in one line |
obrie829 | 15:4efc66de795a | 74 | } |
obrie829 | 15:4efc66de795a | 75 | |
obrie829 | 15:4efc66de795a | 76 | void Brobot::ledShow() |
obrie829 | 15:4efc66de795a | 77 | { |
obrie829 | 15:4efc66de795a | 78 | static int timer = 0; // static is only the first time/loop |
obrie829 | 15:4efc66de795a | 79 | // for( int ii = 0; ii<6; ++ii) |
obrie829 | 15:4efc66de795a | 80 | // leds[ii] = !leds[ii]; |
obrie829 | 15:4efc66de795a | 81 | |
obrie829 | 15:4efc66de795a | 82 | //quit ticker and start led distance show |
obrie829 | 15:4efc66de795a | 83 | if( ++timer > 10) { |
obrie829 | 15:4efc66de795a | 84 | t1.detach(); |
obrie829 | 15:4efc66de795a | 85 | t1.attach( callback(this, &Brobot::ledDistance), 0.01f ); |
obrie829 | 15:4efc66de795a | 86 | } |
obrie829 | 0:eba74e7a229b | 87 | } |
obrie829 | 0:eba74e7a229b | 88 | |
obrie829 | 15:4efc66de795a | 89 | bool Brobot::foundGreenBrick() |
obrie829 | 15:4efc66de795a | 90 | { |
obrie829 | 15:4efc66de795a | 91 | |
obrie829 | 15:4efc66de795a | 92 | if( pixy.objectDetected() ) { |
obrie829 | 15:4efc66de795a | 93 | if( pixy.getSignature() == 1) { // 1 is the green brick |
obrie829 | 15:4efc66de795a | 94 | return true; |
obrie829 | 15:4efc66de795a | 95 | } |
obrie829 | 15:4efc66de795a | 96 | } |
obrie829 | 15:4efc66de795a | 97 | return false; |
obrie829 | 15:4efc66de795a | 98 | } |
obrie829 | 15:4efc66de795a | 99 | |
obrie829 | 15:4efc66de795a | 100 | bool Brobot::foundHome() |
obrie829 | 0:eba74e7a229b | 101 | { |
obrie829 | 15:4efc66de795a | 102 | if( pixy.objectDetected() ) { |
obrie829 | 15:4efc66de795a | 103 | if( pixy.getSignature() == 2) { // 2 is the home |
obrie829 | 15:4efc66de795a | 104 | return true; |
obrie829 | 15:4efc66de795a | 105 | } |
obrie829 | 0:eba74e7a229b | 106 | } |
obrie829 | 15:4efc66de795a | 107 | return false; |
obrie829 | 15:4efc66de795a | 108 | } |
obrie829 | 0:eba74e7a229b | 109 | |
obrie829 | 15:4efc66de795a | 110 | void Brobot::approachBrick() |
obrie829 | 15:4efc66de795a | 111 | { |
obrie829 | 15:4efc66de795a | 112 | // Crystal's code |
obrie829 | 15:4efc66de795a | 113 | float e = 160-pixy.getX(); |
obrie829 | 15:4efc66de795a | 114 | float diff = pixypid.calc( e, 0.005f ); |
obrie829 | 15:4efc66de795a | 115 | speedctrl.setDesiredSpeed( -(diff*50.0f), -(diff*50.0f)) ; |
obrie829 | 15:4efc66de795a | 116 | // function assumes Brick is in open grips |
obrie829 | 0:eba74e7a229b | 117 | } |
obrie829 | 0:eba74e7a229b | 118 | |
obrie829 | 15:4efc66de795a | 119 | bool Brobot::approachHome() |
obrie829 | 8:351b0b7b05b2 | 120 | { |
obrie829 | 15:4efc66de795a | 121 | float x_e, diff ; |
obrie829 | 15:4efc66de795a | 122 | int triggered_sensors = 0, rotate_count = 0; |
obrie829 | 15:4efc66de795a | 123 | bool home_reached = false; |
obrie829 | 15:4efc66de795a | 124 | //the time to rotate after the brick is released and the distance away from |
obrie829 | 15:4efc66de795a | 125 | // the way that the robot should stop at |
obrie829 | 15:4efc66de795a | 126 | float sensor_trigger_dist=0.3; |
obrie829 | 15:4efc66de795a | 127 | |
obrie829 | 15:4efc66de795a | 128 | while(!home_reached) { |
obrie829 | 15:4efc66de795a | 129 | // find the error in the x alignment and calculate the restoring voltage |
obrie829 | 15:4efc66de795a | 130 | x_e = pixy.getX()-160; |
obrie829 | 15:4efc66de795a | 131 | diff = 50*pid.calc(x_e,0.05); |
obrie829 | 15:4efc66de795a | 132 | |
obrie829 | 15:4efc66de795a | 133 | // if home is detected drive the robot forward while continuously realigning the robot |
obrie829 | 15:4efc66de795a | 134 | if (pixy.objectDetected() && (pixy.getSignature() == 2)) { |
obrie829 | 15:4efc66de795a | 135 | speedctrl.setDesiredSpeed(15-diff,-15-diff); |
obrie829 | 15:4efc66de795a | 136 | rotate_count = 0; |
obrie829 | 15:4efc66de795a | 137 | } |
obrie829 | 15:4efc66de795a | 138 | // check the distance sensors. Home is reached when 2 sensors read < 0.3m |
obrie829 | 15:4efc66de795a | 139 | triggered_sensors = (sensor_left<sensor_trigger_dist)+(sensor_right<sensor_trigger_dist)+(sensor_front<sensor_trigger_dist); |
obrie829 | 15:4efc66de795a | 140 | if (triggered_sensors >= 2) |
obrie829 | 15:4efc66de795a | 141 | home_reached = true; |
obrie829 | 15:4efc66de795a | 142 | |
obrie829 | 15:4efc66de795a | 143 | // if home is not detected spin CW and check again |
obrie829 | 15:4efc66de795a | 144 | if (!pixy.objectDetected() || (pixy.objectDetected() && pixy.getSignature() == 1)) { |
obrie829 | 15:4efc66de795a | 145 | Brobot::rotate(5); |
obrie829 | 15:4efc66de795a | 146 | rotate_count++; |
obrie829 | 15:4efc66de795a | 147 | if (rotate_count > 10) |
obrie829 | 15:4efc66de795a | 148 | break; //break the loop if the robot rotates enough times in a row without detecting home |
obrie829 | 15:4efc66de795a | 149 | } |
obrie829 | 15:4efc66de795a | 150 | } |
obrie829 | 15:4efc66de795a | 151 | return home_reached; |
obrie829 | 8:351b0b7b05b2 | 152 | } |
obrie829 | 15:4efc66de795a | 153 | |
obrie829 | 15:4efc66de795a | 154 | void Brobot::closeGrip() |
obrie829 | 15:4efc66de795a | 155 | { |
obrie829 | 15:4efc66de795a | 156 | int close=600; |
obrie829 | 15:4efc66de795a | 157 | servo.write_us(close); |
obrie829 | 15:4efc66de795a | 158 | } |
obrie829 | 15:4efc66de795a | 159 | |
obrie829 | 15:4efc66de795a | 160 | void Brobot::openGrip() |
obrie829 | 15:4efc66de795a | 161 | { |
obrie829 | 15:4efc66de795a | 162 | int open=2100; |
obrie829 | 15:4efc66de795a | 163 | servo.write_us(open); |
obrie829 | 15:4efc66de795a | 164 | } |
obrie829 | 15:4efc66de795a | 165 | |
obrie829 | 15:4efc66de795a | 166 | |
obrie829 | 15:4efc66de795a | 167 | void Brobot::rotate(int phi) |
obrie829 | 15:4efc66de795a | 168 | { |
obrie829 | 15:4efc66de795a | 169 | if(phi>25|| phi<-25) { |
obrie829 | 15:4efc66de795a | 170 | phi=10; |
obrie829 | 15:4efc66de795a | 171 | } |
obrie829 | 15:4efc66de795a | 172 | speedctrl.setDesiredSpeed(phi, phi); |
obrie829 | 15:4efc66de795a | 173 | } |
obrie829 | 8:351b0b7b05b2 | 174 | |
obrie829 | 0:eba74e7a229b | 175 | void Brobot::forward() |
obrie829 | 0:eba74e7a229b | 176 | { |
obrie829 | 15:4efc66de795a | 177 | speedctrl.setDesiredSpeed(20, -20); // rpms |
obrie829 | 0:eba74e7a229b | 178 | } |
obrie829 | 0:eba74e7a229b | 179 | |
obrie829 | 0:eba74e7a229b | 180 | void Brobot::back() |
obrie829 | 0:eba74e7a229b | 181 | { |
obrie829 | 15:4efc66de795a | 182 | speedctrl.setDesiredSpeed(-15, 15); //rpms |
obrie829 | 0:eba74e7a229b | 183 | } |
obrie829 | 0:eba74e7a229b | 184 | |
obrie829 | 0:eba74e7a229b | 185 | void Brobot::stop() |
obrie829 | 0:eba74e7a229b | 186 | { |
obrie829 | 15:4efc66de795a | 187 | speedctrl.setDesiredSpeed(0.0f, 0.0f); |
obrie829 | 0:eba74e7a229b | 188 | } |