File content as of revision 0:ac150fd4158e:

#include "Movement.h"

PwmOut steering(p21);
PwmOut velocity(p22);

float forwardSpeed = 0.3, reverseSpeed = -0.3, turnAngleRight = 1, turnAngleLeft = -1, vo = 0.0;

void Velocity(float v) {
    v=v+1;
    if (v>=0 && v<=2) {
        if (vo>=1 && v<1) {
            velocity.pulsewidth(0.0014);
            wait(0.1);
            velocity.pulsewidth(0.0015);    // move into reverseSpeed
            wait(0.1);
        }
        velocity.pulsewidth(v/2000+0.001);
        vo=v;
    }
}

// Steering expects -1 (left) to +1 (right)
void Steering(float s) {
    s=s+1;
    if (s>=0 && s<=2) {
        steering.pulsewidth(s/2000+0.001);
    }
}

//Always run this function!! Enables you to drive
void setUpMovement(const float &forwardSpeedInput, const float &reverseSpeedInput, const float &turnAngleInput){
    forwardSpeed = forwardSpeedInput;
    reverseSpeed = reverseSpeedInput;

    if(turnAngleInput > 0)
        turnAngleRight = turnAngleInput;
    else
        turnAngleRight *= turnAngleInput;

    turnAngleLeft = (float)(turnAngleRight*-1.0);

    velocity.period(0.02);
    steering.period(0.02);
    Velocity(0); // initiate the drive motor (this must be done)
    Steering(0); // centre steering
    wait(0.5);
}


void moveForward() {
    Velocity(forwardSpeed);
    wait(0.1);
}

void moveReverse() {
    Velocity(reverseSpeed);
    wait(0.1);
}

void move(const float &speed) {
    Velocity(reverseSpeed);
    wait(0.1);
}


void steerLeft() {
    Steering(turnAngleLeft);
    wait(0.2);
}

void steerRight() {
    Steering(turnAngleRight);
    wait(0.2);
}

void steer(const float &angle) {
    Steering(turnAngleRight);
    wait(0.2);
}


void setReverseSpeed(const float &speed) {
    reverseSpeed = speed;
}

void setForwardSpeed(const float &speed) {
    forwardSpeed = speed;
}

void setTurnAngle(const float &angle) {
    if(angle>=0)
        turnAngleRight = angle;
    else
        turnAngleRight = angle*-1;
    
    turnAngleLeft = turnAngleRight*1;
}

void stop() {
    Velocity(0.0);
    wait(0.5);
}

void resetVelocity() {
    Velocity(0.0);
    wait(0.5);
}

void resetSteering() {
    Steering(0);
    wait(0.5);
}

void resetAll() {
    Velocity(0); // initiate the drive motor (this must be done)
    Steering(0); // centre steering
    wait(0.5);
}