File content as of revision 8:ce5b1bf38077:

#ifndef MOTOR_H
#define MOTOR_H

#include "mbed.h"
#include "Mapping.h"

#define turn_time 152
#define MOTOR_WAIT 600
#define forward_time 190


//#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);
    
    
    
    stop_mov();
}

void turn_right(int r)
{
    
    wait(0.5);
    forward(3);
    wait_ms(320);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(-2);
    wait_us(MOTOR_WAIT);
    setRightSpeed(-1);
    wait_ms(25);
    stop_mov();
    

}
void turn_left(int l)
{
    forward(2);
    wait_ms(200);
    stop_mov();
    setLeftSpeed(-l);
    wait_us(MOTOR_WAIT);
    setRightSpeed(l);
    wait_ms(turn_time);       ///Set for turn speed
    setRightSpeed(0);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(0);
    wait_us(MOTOR_WAIT);
    orientation_turnLeft();
    forward(2);
    wait_ms(200);
    stop_mov();
}

void turn_around(int a)
{
    setLeftSpeed(-a);
    wait_us(MOTOR_WAIT);
    setRightSpeed(a);
    wait_ms(turn_time);  
    wait_ms(turn_time);     ///Set for turn speed
    setRightSpeed(0);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(0);
    wait_us(MOTOR_WAIT);
    orientation_turnAround();
}

void handbrake()
{
    if(lSpeed > 0 && rSpeed > 0)
    {
    wait_us(MOTOR_WAIT);
    setLeftSpeed(-3);
    wait_us(MOTOR_WAIT);
    setRightSpeed(-4);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(-4);
    wait_ms(50);
    setRightSpeed(-3);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(-4);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(-3);
    wait_ms(100);
    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);
    wait_us(MOTOR_WAIT);
    setRightSpeed(0);
    wait_us(MOTOR_WAIT);
    setLeftSpeed(0);
}
#endif