File content as of revision 3:1a8a7cc709cc:

#ifndef MOTOR_H
#define MOTOR_H

#include "mbed.h"

#define turn_time 152
#define MOTOR_WAIT 500
#define forward_time 190

#include "PID.h"
//#include "Sensors.h"

Serial Motor(p13,p14);

int rSpeed = 0;
int lSpeed = 0;

void setRightSpeed(int speed)
{
    if(speed >= 0)
        Motor.printf("2f%i\r", speed);
    else
        Motor.printf("2r%i\r", (-speed));
        
        
    rSpeed = speed;
    wait_us(MOTOR_WAIT);
}


void setLeftSpeed(int speed)
{
    if(speed >= 0)
        Motor.printf("1r%i\r", speed);
    else
        Motor.printf("1f%i\r", (-speed));
        
    lSpeed = speed;
    wait_us(MOTOR_WAIT);
}

void stop_mov()
{
    wait_us(MOTOR_WAIT);
    setLeftSpeed(0);
    wait_us(MOTOR_WAIT);
    setRightSpeed(0);
    wait_us(MOTOR_WAIT);
  
}

void forward(int f)
{
    wait_us(MOTOR_WAIT);
    setLeftSpeed(f-1);
    wait_us(MOTOR_WAIT);
    setRightSpeed(f);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(f);
    wait_us(MOTOR_WAIT);
    
    wait_ms(1000);
    
    stop_mov();
}

void turn_right(int r)
{
    setLeftSpeed(r);
    wait_us(MOTOR_WAIT);
    setRightSpeed(-r);
    wait_ms(MOTOR_WAIT);       ///Set for turn speed
    setLeftSpeed(-r);
    wait_us(MOTOR_WAIT);
    setRightSpeed(r);
    wait_ms(100);
    stop_mov();
    
    stop_mov();
}
void turn_left(int l)
{
    setLeftSpeed(-l);
    wait_us(MOTOR_WAIT);
    setRightSpeed(l);
    wait_ms(MOTOR_WAIT);       ///Set for turn speed
    setRightSpeed(0);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(0);
    wait_us(MOTOR_WAIT);
}
void handbrake()
{
    wait_us(MOTOR_WAIT);
    setLeftSpeed(-3);
    wait_us(MOTOR_WAIT);
    setRightSpeed(-4);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(-4);
    wait_ms(10);
    setRightSpeed(-3);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(-4);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(-3);
    wait_ms(10);
    wait_us(MOTOR_WAIT);
    setRightSpeed(-1);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(-1);
    wait_ms(10);
    wait_us(MOTOR_WAIT);
    setRightSpeed(0);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(0);
}
#endif