File content as of revision 0:2ab9f4a3187a:

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

ParallaxRobotShield::ParallaxRobotShield(PinName leftPin, PinName rightPin) : leftServo(leftPin) , rightServo(rightPin)
{
    leftPosition = 1450;
    rightPosition = 1550;    
}

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


void ParallaxRobotShield::right_servo(int speed)
{
    speed = 1490 + (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::Enable(int StartPos, int Period) 
//{
//    leftPosition = StartPos;
//    leftPulse.attach_us(this, &ParallaxRobotShield::enable_left_motor, Period);
//}