File content as of revision 2:118efce95f3c:

#include "SeeedStudioShieldBot.h"
#include "mbed.h"


SeeedStudioShieldBot::SeeedStudioShieldBot() :  rightSensor(PTB0),
    inRightSensor(PTB1),
    centreSensor(PTB2),
    inLeftSensor(PTB3),
    leftSensor(PTA4),
    motor1A(PTA5),
    motor1B(PTC9),
    motor1En(PTC8),
    motor2A(PTA12),
    motor2B(PTD0),
    motor2En(PTD5) {
    // Something should go here...
}

void SeeedStudioShieldBot::right_motor(float speed) { 
    // The bit that's actually needed...
    if (speed >= 0) {
        motor1A = speed;
        motor1B = 0;
    }
    else {
        motor1A = 1 + speed;
        motor1B = 1;
    }
}

void SeeedStudioShieldBot::left_motor(float speed) {   
    // Useful bit
    if (speed >= 0) {
        motor2A = speed;
        motor2B = 0;
    }
    else {
        motor2A = 1 + speed;
        motor2B = 1;
    }
}

// The following two functions turn the robot on the spot...

void SeeedStudioShieldBot::left(float speed) {
    left_motor(-speed);
    right_motor(speed);
}

void SeeedStudioShieldBot::right(float speed) {
    left_motor(speed);
    right_motor(-speed);
}

// Again, until we get PWM on the pin w/o PWM, there's no way we can really have this any other way than 1...
// This is the case for both forwards and backwards...

// FIXED, using jumper between pins 8 and 3 on top of the robot board... unfortunately it wastes an io pin, but is easier than using software IO.
// It also means that the 'lid' can't go on top.

void SeeedStudioShieldBot::forward(float speed) {
    if (speed == 0) {
        left_motor(0);
        right_motor(0);
    }
    else {
        left_motor(speed);
        right_motor(speed);
    }
}

void SeeedStudioShieldBot::backward(float speed) {
    if (speed == 0) {
        left_motor(0);
        right_motor(0);
    }
    else {
        left_motor(-speed);
        right_motor(-speed);
    }
}

void SeeedStudioShieldBot::enable_right_motor() {
    motor1En = 1;
}

void SeeedStudioShieldBot::enable_left_motor() {
    motor2En = 1;
}

void SeeedStudioShieldBot::disable_right_motor() {
    motor1En = 0;
}

void SeeedStudioShieldBot::disable_left_motor() {
    motor2En = 0;
}

// Give a value representative of the overall data received by the sensors...
// Output between +1 and -1.
// Positive is right, negative left...
float SeeedStudioShieldBot::line_position() {
    float output = 0; 
    if (rightSensor == 1) {
        output += 0.5;
    }
    if (inRightSensor == 1) {
        output += 0.5;
    }
    if (leftSensor == 1) {
        output -= 0.5;
    }
    if (inLeftSensor == 1) {
        output -= 0.5;
    }
    
    return output;
}

void SeeedStudioShieldBot::stopAll() {
    motor1A = 0;
    motor1B = 0;
    motor2A = 0;
    motor2B = 0;
}

void SeeedStudioShieldBot::stop(int motor) {
    if (motor == 1) {
        // Stop motor 0...
        motor1A = 0;
        motor1B = 0;
    }
    else if (motor == 2) {
        motor2A = 0;
        motor2B = 0;
    }
}