File content as of revision 0:de792000cd8b:

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

//ParallaxRobotShield::ParallaxRobotShield(PinName leftServoPin, PinName rightServoPin) : leftServo(leftServoPin) , rightServo(rightServoPin) , leftWhisker(NULL) , rightWhisker(NULL)
//{
//    leftPosition = 1450;
//    rightPosition = 1550;    
//}

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

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


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

//void ParallaxRobotShield::StartPulse() 
//{
//    ServoPin = 1;
//    PulseStop.attach_us(this, &ParallaxRobotShield::EndPulse, Position);
//}

void ParallaxRobotShield::disable_left_motor()  // Just for internal functions
{
    leftServo = 0;
}
//void ParallaxRobotShield::EndPulse() 
//{
//    ServoPin = 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::turn_left(int speed)
{
    stopRight();
    int i;
    for(i = 0; i < 100; i++)
    {
        left_servo(speed);
        wait_ms(10);    
    }
    enable_right_motor();
}


void ParallaxRobotShield::turn_right(int speed)
{
    stopLeft();
    int i;
    for(i = 0; i < 100; i++)
    {
        right_servo(speed);
        wait_ms(10);    
    }
    enable_left_motor();
}


void ParallaxRobotShield::forward(int speed)
{
    int i;
    for(i = 0; i < 100; i++)
    {
        left_servo(speed);
        right_servo(speed); 
        wait_ms(20);
    }
}


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


void ParallaxRobotShield::left(int speed)
{
    int i;
    for(i = 0; i < 100; i++)
    {
        left_servo(speed);
        right_servo(-speed); 
        wait_ms(20);
    }
}


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


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

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


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


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


//void ParallaxRobotShield::whiskersLED()
//{
//    DigitalOut leftLED(PA_10);
//    DigitalOut rightLED(PA_5);
//    while(leftWhiskerContact())
//        leftLED = 1;
//    while(rightWhiskerContact())
//        rightLED = 1;    
//    leftLED = 0;
//    rightLED = 0;
//}

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


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


//void ParallaxRobotShield::whisker()
//{
//    DigitalOut leftLED(PA_10);
//    DigitalOut rightLED(PA_5);
//    while(1)
//    {
//        if(leftWhisker.read() == 0)
//        {
//            leftLED = 1;
//            leftWhiskerContact();
//        }
//        leftLED = 0;
//        if(rightWhisker.read() == 0)
//        {
//            rightLED = 1;
//            rightWhiskerContact();
//        }
//        rightLED = 0;
//        left_servo(100);
//        right_servo(100);
//    }
//}




//void ParallaxRobotShield::Enable(int StartPos, int Period) 
//{
//    leftPosition = StartPos;
//    leftPulse.attach_us(this, &ParallaxRobotShield::enable_left_motor, Period);
//}