Mechatronics Robotics / Mbed 2 deprecated BrobotV1

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

Dependencies:   mbed UniServ

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?

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