File content as of revision 17:ec52258b9472:

/*
 * BROBOT.cpp
 *
 */

#include <cmath>
#include "Brobot.h"

Brobot::Brobot(SpeedControl& speedctrl_, AnalogIn& distance_, DigitalOut& enable_, DigitalOut& bit0_, DigitalOut& bit1_, DigitalOut& bit2_, DigitalOut* led_ptr, Pixy& pixy_, UniServ& servo_) :
    speedctrl(speedctrl_), distance(distance_), enable(enable_), bit0(bit0_), bit1(bit1_), bit2(bit2_), leds(led_ptr), pixy(pixy_), servo(servo_) // assigning parameters to class variables
{
    //initialize distance sensors
    for( int ii = 0; ii<6; ++ii)
        sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);

    //defining the sensors for
    sensor_front.init( &distance, &bit0, &bit1, &bit2, 0);  // & give the address of the object
    sensor_left.init( &distance, &bit0, &bit1, &bit2, 5);
    sensor_right.init( &distance, &bit0, &bit1, &bit2, 1);

    //                      kp        ki       kd      min     max
    //pid.setPIDValues(      0.025f,    0.00005f,  0.001f,   0.5f, -0.5f, 1000);
    pid.setPIDValues(      0.4f,    0.001f, 0.001f, 0.5f, -0.5f, 1000);
    pixypid.setPIDValues(  0.005f, 0.0f, 0.000f, 25.0f, -25.0f, 1000);
    //pixypidS.setPIDValues( 0.01f,   0.0f,   0.0f, 20.0f,-20.0f,   1000);
    //e = 0 ;

}

void Brobot::avoidObstacleAndMove(int vtrans)
{
    float vrot = 0;   // rpm

    if(sensor_left.read() <0.3f || sensor_right.read() <0.3f) { // to avoid obstacles

        e = sensor_left.read() - sensor_right.read();
        float diff = pid.calc( e, 0.05f );  // error and period are arguments

        vrot = diff*50; //turn

        if( vrot<= -10)
            vrot=-10;
        else if (vrot > 10)
            vrot=10;
    }

    speedctrl.setDesiredSpeed( vtrans - vrot, -vtrans - vrot );

    if(sensor_front.read() <=0.25f) { // when approaching normal to wall
        speedctrl.setDesiredSpeed( 0, 0 );

        int direction=rand()%2 ;    // so there is variablility in the robots path
        if(direction==0) {
            speedctrl.setDesiredSpeed(5, 5 );
            wait(0.5);
        } else if (direction==1) {
            speedctrl.setDesiredSpeed( -5, -5 );
            wait(0.5);
        }
    }
}

void Brobot::ledDistance()
{
    for( int ii = 0; ii<6; ++ii)
        sensors[ii]< 0.1f ? leds[ii].write(1) : leds[ii].write(0);   // an if statement in one line
    //printf("the L and R sensor values are %f and %f \n\r", sensor_left, sensor_right);
}

void Brobot::ledShow()
{
    static int timer = 0;   // static is only the first time/loop
    // for( int ii = 0; ii<6; ++ii)
    //   leds[ii] = !leds[ii];

    //quit ticker and start led distance show
    if( ++timer > 10) {
        t1.detach();
        t1.attach( callback(this, &Brobot::ledDistance), 0.05f );
    }
}

bool Brobot::foundGreenBrick()
{
    if( pixy.objectDetected() ) {
        if( pixy.getSignature() == 1) { // 1 is the green brick
            return true;
        }
    }
    return false;
}

bool Brobot::foundHome()
{
    if( pixy.objectDetected() ) {
        if( pixy.getSignature() == 2) {  // 2 is the red home
            return true;
        }
    }
    return false;
}

int Brobot::rotateAndApproach()
{
    static float objectDetected = 1.0f;  // assumes we can already detect object

    objectDetected = 0.95f * objectDetected + 0.05f * pixy.objectDetected();// filtering for flickering pixy

    if( pixy.getSignature() == 1 && objectDetected > 0.2f) {

        leds[3] = !leds[3] ;

        float pixyX = pixy.getX();
        float pixyY = pixy.getY();
        float error = 160-pixyX;
        float vrot = error * 0.25f ;
        float vtrans = (200.0f - pixyY) * 0.25f;

        if( vrot > 5) vrot = 5;
        if( vrot < -5) vrot = -5;
        if( vtrans > 5) vtrans = 5;
        if( vtrans < -5) vtrans = -5;

        //speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
        speedctrl.setDesiredSpeed( -vrot , -vrot ) ;

        if ((error < 15)&&(error>-15)) {
            speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
        }

        if( pixyY >= 180) { // assumes centered on brick
            speedctrl.setDesiredSpeed(8.0f, -8.0f); // blindly drive forward
            wait(0.5);
            Brobot::closeGrip();
            return 1; // need to transfer to GoHome state
        }
        return 3;  // continue the approach function
    } else return 2; // return to search state, no object is detected
}

bool Brobot::approachHome()
{
    static float objectDetected = 1.0f;  // assumes we can already detect object

    objectDetected = 0.98f * objectDetected + 0.02f * pixy.objectDetected();// filtering for flickering pixy
    leds[4]=0;
    if(objectDetected>0.1f) leds[4]=1;
    
    if( (pixy.getSignature() == 2) && (objectDetected > 0.1f)) {

        float pixyX = pixy.getX();
        float pixyY = pixy.getY();
        float error = 160-pixyX;
        float vrot = error * 0.25f ;
        float vtrans = (200.0f - pixyY) * 0.25f;


        if( pixyY >= 50) return true; // need to transfer to atHome state
        if( vrot > 4) vrot = 4;
        if( vrot < -4) vrot = -4;
        if( vtrans > 4) vtrans = 4;
        if( vtrans < -4) vtrans = -4;

        //speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
        speedctrl.setDesiredSpeed( -vrot , -vrot ) ;

        if ((error <= 25) && (error>= -25)) {
            speedctrl.setDesiredSpeed( vtrans -vrot , -vtrans -vrot ) ;
        }

        return false; 
    } else return false; // return to goHome case, no object is detected
}

void Brobot::closeGrip()
{
    int close=1400;
    servo.write_us(close);
    wait(0.5);
}

void Brobot::openGrip()
{
    int open=1900;
    servo.write_us(open);
    wait(0.5);
}


void Brobot::rotate(int phi)
{
    if(phi>25|| phi<-25) {
        phi=10;
    }
    speedctrl.setDesiredSpeed(phi, phi);
}

void Brobot::forward()
{
    speedctrl.setDesiredSpeed(20, -20);  // rpms
}

void Brobot::back()
{
    speedctrl.setDesiredSpeed(-15, 15);  //rpms
}

void Brobot::stop()
{
    speedctrl.setDesiredSpeed(0.0f, 0.0f);
}