Brobot.cpp

00001 /*
00002  * BROBOT.cpp
00003  *
00004  */
00005 
00006 #include <cmath>
00007 #include "Brobot.h"
00008 
00009 Brobot::Brobot(SpeedControl& speedctrl_, AnalogIn& distance_, DigitalOut& enable_, DigitalOut& bit0_, DigitalOut& bit1_, DigitalOut& bit2_, DigitalOut* led_ptr, Pixy& pixy_, UniServ& servo_) :
00010     speedctrl(speedctrl_), distance(distance_), enable(enable_), bit0(bit0_), bit1(bit1_), bit2(bit2_), leds(led_ptr), pixy(pixy_), servo(servo_) // assigning parameters to class variables
00011 {
00012     //initialize distance sensors
00013     for( int ii = 0; ii<6; ++ii)
00014         sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
00015 
00016     //defining the sensors for
00017     sensor_front.init( &distance, &bit0, &bit1, &bit2, 0);  // & give the address of the object
00018     sensor_left.init( &distance, &bit0, &bit1, &bit2, 5);
00019     sensor_right.init( &distance, &bit0, &bit1, &bit2, 1);
00020 
00021     //                      kp        ki       kd      min     max
00022     //pid.setPIDValues(      0.025f,    0.00005f,  0.001f,   0.5f, -0.5f, 1000);
00023     pid.setPIDValues(      0.4f,    0.001f, 0.001f, 0.5f, -0.5f, 1000);
00024     pixypid.setPIDValues(  0.005f, 0.0f, 0.000f, 25.0f, -25.0f, 1000);
00025     //pixypidS.setPIDValues( 0.01f,   0.0f,   0.0f, 20.0f,-20.0f,   1000);
00026     //e = 0 ;
00027 
00028 }
00029 
00030 void Brobot::avoidObstacleAndMove(int vtrans)
00031 {
00032     float vrot = 0;   // rpm
00033 
00034     if(sensor_left.read() <0.3f || sensor_right.read() <0.3f) { // to avoid obstacles
00035 
00036         e = sensor_left.read() - sensor_right.read();
00037         float diff = pid.calc( e, 0.05f );  // error and period are arguments
00038 
00039         vrot = diff*50; //turn
00040 
00041         if( vrot<= -10)
00042             vrot=-10;
00043         else if (vrot > 10)
00044             vrot=10;
00045     }
00046 
00047     speedctrl.setDesiredSpeed( vtrans - vrot, -vtrans - vrot );
00048 
00049     if(sensor_front.read() <=0.25f) { // when approaching normal to wall
00050         speedctrl.setDesiredSpeed( 0, 0 );
00051 
00052         int direction=rand()%2 ;    // so there is variablility in the robots path
00053         if(direction==0) {
00054             speedctrl.setDesiredSpeed(5, 5 );
00055             wait(0.5);
00056         } else if (direction==1) {
00057             speedctrl.setDesiredSpeed( -5, -5 );
00058             wait(0.5);
00059         }
00060     }
00061 }
00062 
00063 void Brobot::ledDistance()
00064 {
00065     for( int ii = 0; ii<6; ++ii)
00066         sensors[ii]< 0.1f ? leds[ii].write(1) : leds[ii].write(0);   // an if statement in one line
00067     //printf("the L and R sensor values are %f and %f \n\r", sensor_left, sensor_right);
00068 }
00069 
00070 void Brobot::ledShow()
00071 {
00072     static int timer = 0;   // static is only the first time/loop
00073     // for( int ii = 0; ii<6; ++ii)
00074     //   leds[ii] = !leds[ii];
00075 
00076     //quit ticker and start led distance show
00077     if( ++timer > 10) {
00078         t1.detach();
00079         t1.attach( callback(this, &Brobot::ledDistance), 0.05f );
00080     }
00081 }
00082 
00083 bool Brobot::foundGreenBrick()
00084 {
00085     if( pixy.objectDetected() ) {
00086         if( pixy.getSignature() == 1) { // 1 is the green brick
00087             return true;
00088         }
00089     }
00090     return false;
00091 }
00092 
00093 bool Brobot::foundHome()
00094 {
00095     if( pixy.objectDetected() ) {
00096         if( pixy.getSignature() == 2) {  // 2 is the red home
00097             return true;
00098         }
00099     }
00100     return false;
00101 }
00102 
00103 int Brobot::rotateAndApproach()
00104 {
00105     static float objectDetected = 1.0f;  // assumes we can already detect object
00106 
00107     objectDetected = 0.95f * objectDetected + 0.05f * pixy.objectDetected();// filtering for flickering pixy
00108 
00109     if( pixy.getSignature() == 1 && objectDetected > 0.2f) {
00110 
00111         leds[3] = !leds[3] ;
00112 
00113         float pixyX = pixy.getX();
00114         float pixyY = pixy.getY();
00115         float error = 160-pixyX;
00116         float vrot = error * 0.25f ;
00117         float vtrans = (200.0f - pixyY) * 0.25f;
00118 
00119         if( vrot > 5) vrot = 5;
00120         if( vrot < -5) vrot = -5;
00121         if( vtrans > 5) vtrans = 5;
00122         if( vtrans < -5) vtrans = -5;
00123 
00124         //speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
00125         speedctrl.setDesiredSpeed( -vrot , -vrot ) ;
00126 
00127         if ((error < 15)&&(error>-15)) {
00128             speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
00129         }
00130 
00131         if( pixyY >= 180) { // assumes centered on brick
00132             speedctrl.setDesiredSpeed(8.0f, -8.0f); // blindly drive forward
00133             wait(0.5);
00134             Brobot::closeGrip();
00135             return 1; // need to transfer to GoHome state
00136         }
00137         return 3;  // continue the approach function
00138     } else return 2; // return to search state, no object is detected
00139 }
00140 
00141 bool Brobot::approachHome()
00142 {
00143     static float objectDetected = 1.0f;  // assumes we can already detect object
00144 
00145     objectDetected = 0.98f * objectDetected + 0.02f * pixy.objectDetected();// filtering for flickering pixy
00146     leds[4]=0;
00147     if(objectDetected>0.1f) leds[4]=1;
00148     
00149     if( (pixy.getSignature() == 2) && (objectDetected > 0.1f)) {
00150 
00151         float pixyX = pixy.getX();
00152         float pixyY = pixy.getY();
00153         float error = 160-pixyX;
00154         float vrot = error * 0.25f ;
00155         float vtrans = (200.0f - pixyY) * 0.25f;
00156 
00157 
00158         if( pixyY >= 50) return true; // need to transfer to atHome state
00159         if( vrot > 4) vrot = 4;
00160         if( vrot < -4) vrot = -4;
00161         if( vtrans > 4) vtrans = 4;
00162         if( vtrans < -4) vtrans = -4;
00163 
00164         //speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
00165         speedctrl.setDesiredSpeed( -vrot , -vrot ) ;
00166 
00167         if ((error <= 25) && (error>= -25)) {
00168             speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
00169         }
00170 
00171         return false; 
00172     } else return false; // return to goHome case, no object is detected
00173 }
00174 
00175 void Brobot::closeGrip()
00176 {
00177     int close=1400;
00178     servo.write_us(close);
00179     wait(0.5);
00180 }
00181 
00182 void Brobot::openGrip()
00183 {
00184     int open=1900;
00185     servo.write_us(open);
00186     wait(0.5);
00187 }
00188 
00189 
00190 void Brobot::rotate(int phi)
00191 {
00192     if(phi>25|| phi<-25) {
00193         phi=10;
00194     }
00195     speedctrl.setDesiredSpeed(phi, phi);
00196 }
00197 
00198 void Brobot::forward()
00199 {
00200     speedctrl.setDesiredSpeed(20, -20);  // rpms
00201 }
00202 
00203 void Brobot::back()
00204 {
00205     speedctrl.setDesiredSpeed(-15, 15);  //rpms
00206 }
00207 
00208 void Brobot::stop()
00209 {
00210     speedctrl.setDesiredSpeed(0.0f, 0.0f);
00211 }