File content as of revision 0:0fd8d387c5a0:

#include "ParallaxRobotShield.h"
#include "mbed.h"


ParallaxRobotShield::ParallaxRobotShield(PinName leftServoPin, PinName rightServoPin, PinName leftWhiskerPin, PinName rightWhiskerPin) : 
                     leftServo(leftServoPin) , rightServo(rightServoPin) , leftWhisker(leftWhiskerPin) , rightWhisker(rightWhiskerPin), leftServo1(leftServoPin) , rightServo1(rightServoPin)
{
    leftPosition = 1500;
    rightPosition = 1500;
}

void ParallaxRobotShield::left_servo(int speed)
{
    speed = 1500 - (speed * 2);
    leftPosition = speed;
}


void ParallaxRobotShield::right_servo(int speed)
{
    speed = 1500 + (speed * 2);
    rightPosition = speed;
}


void ParallaxRobotShield::forward(int speed, float time)
{
    backward(-speed, time);
}


void ParallaxRobotShield::backward(int speed, float time)
{
    time = time * 1000;         // conversion to milliseconds
    Timer t;
    t.reset();
    t.start();
    while(t.read_ms() != time)
    {
        left_servo(speed);
        right_servo(speed); 
    }
    t.stop();
}


void ParallaxRobotShield::left(int speed, float time)
{
    time = time * 1000;         // conversion to milliseconds
    Timer t;
    t.reset();
    t.start();
    while(t.read_ms() != time)
    {
        left_servo(speed);
        right_servo(-speed); 
    }
    t.stop();
}


void ParallaxRobotShield::right(int speed, float time)
{
    left(-speed, time);
}


void ParallaxRobotShield::turn_left(int speed, float time)
{
    time = time * 1000;         // conversion to milliseconds
    stopRight();
    Timer t;
    t.reset();
    t.start();
    while(t.read_ms() != time)
        left_servo(speed);  
    t.stop();
    enable_right_motor();
}


void ParallaxRobotShield::turn_right(int speed, float time)
{
    time = time * 1000;         // conversion to milliseconds
    stopLeft();
    Timer t;
    t.reset();
    t.start();
    while(t.read_ms() != time)
        right_servo(speed);
    t.stop();
    enable_left_motor();
}


void ParallaxRobotShield::disable_left_motor()  // Just for internal functions
{
    leftServo = 0;
}


void ParallaxRobotShield::disable_right_motor() // Just for internal functions
{
    rightServo = 0;
}


void ParallaxRobotShield::enable_left_motor()
{
    leftServo = 1;
    leftPulse.attach_us(this, &ParallaxRobotShield::enable_left_motor, 20000);      // Period is set to 20000 us by default which is equal to 50 Hz
    leftPulseStop.attach_us(this, &ParallaxRobotShield::disable_left_motor, leftPosition);
}


void ParallaxRobotShield::enable_right_motor()
{
    rightServo = 1;
    rightPulse.attach_us(this, &ParallaxRobotShield::enable_right_motor, 20000);    // Period is set to 20000 us by default which is equal to 50 Hz
    rightPulseStop.attach_us(this, &ParallaxRobotShield::disable_right_motor, rightPosition);
}


void ParallaxRobotShield::stop(int motor)
{
    if(motor == 1)
        stopRight();
    else if(motor == 2)
        stopLeft();
}


void ParallaxRobotShield::stopLeft()
{
    leftPulse.detach();
}


void ParallaxRobotShield::stopRight()
{
    rightPulse.detach();
}


void ParallaxRobotShield::stopAll()
{
    stopRight();
    stopLeft();
}


int ParallaxRobotShield::leftWhiskerContact()
{
    if(leftWhisker.read() == 0)
        return 1;
    else     
        return 0;
}


int ParallaxRobotShield::rightWhiskerContact()
{
    if(rightWhisker.read() == 0)
        return 1;
    else     
        return 0;
}


int ParallaxRobotShield::lightSensor(PinName lightSensor)
{
    DigitalInOut QTR(lightSensor);       // the QTR-1RC light sensor
    int black;
    Timer t;
    QTR.output();
    QTR = 1;
    t.start();
    QTR.input();
    while (QTR == 1 || t.read_us() < 3000);
    black = t.read_us();        
    t.stop();
    t.reset();
        
    if(black > 3100)       // Black
        return 1; 
    else 
        return 0; 
}


int ParallaxRobotShield::bumpSensor(PinName bumpSensor)
{
    DigitalIn bump(bumpSensor);       // the 10 cm Sharp GP2Y0D810Z0F Digital Distance Sensor
    if(bump.read() == 0)
        return 1;
    else
        return 0;
}


double ParallaxRobotShield::distanceSensor(PinName trigPin, PinName echoPin)
{
    DigitalOut trig(trigPin);       // Sends out an Ultasonicsonic signal
    DigitalIn echo(echoPin);        // Recieves an Ultasonicsonic signal
    Timer timer;        // for measuring the duration of the signal's joyless journey
    
    timer.reset();
    trig = 0;
    wait_us(2);
    trig = 1;
    wait_us(10);
    trig = 0;
    while(echo == 0);
    timer.start();
    while(echo == 1);
    timer.stop();

    // Distance = ( (Duration of travel) * (Speed of Ultasonicsonic: 340m/s) ) / 2
    return (timer.read_us() * 0.034) / 2.0;        // Microseconds are used for accuracy
}


/** Optional implementation for future use
void ParallaxRobotShield::whiskersLED(PinName left_LED, PinName right_LED)
{
    DigitalOut leftLED(left_LED);
    DigitalOut rightLED(right_LED);
    if(leftWhiskerContact())
    {
        leftLED = 1;
        wait(0.1);
    }
    if(rightWhiskerContact())
    {
       rightLED = 1;    
       wait(0.1);
    }
    leftLED = 0;
    rightLED = 0;
}
*/