/*Written By Andrew J. Wirshbron-Lucker*/
/*This file contains the functions needed to carry out movement on the Power Wheels Robot. 
Each function takes an int [1, 100] as a value to determin how fast the robot should move. 
This allows an avoidence or seeking functions that require movement to get a more percise 
turn then by demanding that all movement is always done at 100% power. Each movement function
will return a Boolean value regarding which direction the motors are curently set to. This 
value should be checked before any movement functions are called, by using a simple 'if' statement.
Therefor the caller is responcibal for first calling the 'stop()' function before any direction
changes are made. The stop function also takes care of timming by setting a variable called 
'pause' this variable is to be used as a spin lock so no one else can try to access the motors before
the 3 second pause needed to discharge the EMF/EMI that can blow the MOSFETS.

usage examples: 
 
forwad(50);  Robot will move forward at 50% power
right(25);   Robot will make a right turn at 25% power
back(35);    Robot will move backwards at 35% power

How to make more advanced movements
here is an example to combine several of the functions to exceute a 3-point turn.

stop(); 
rev_left(15);
sleep(5);
stop();
right(15);
sleep(5);
*/

#use "timer_lib.ic"

#define MOTOR 1     /*Motor 1 is used for Throttle*/
#define DIRECTION 3 /*Motor 3 is used for the Reallys where fd = forward and off = reverse*/
#define CENTER 2450 /*These are the values for the steering servo*/
#define LEFT  4300
#define RIGHT 600

int lastmove = 0;
int isstopped = 1;
//turtle movements


int isStopped()
{
    return isstopped;
}

int forward(int speed){                 
    //printf("forward\n");        
    Prep_Forward();
    
    servo0 = CENTER;           
    fd(DIRECTION);
    motor(MOTOR, speed);
    return 1;
}

int back(int speed){
    
    //printf("back\n");
    Prep_Back();
    servo0 = CENTER;
    off(DIRECTION);
    motor(MOTOR, speed);
    lastmove = 0;
    isstopped = 0;
    return 0;
}

int right(int speed){
    
    //printf("right\n");
    Prep_Forward();
    servo0 = RIGHT;
    fd(DIRECTION);
    motor(MOTOR, speed);
    lastmove = 1; 
    isstopped = 0;
    return 1;
}

int rev_right(int speed){
    
    //printf("Reverse Right\n");
    Prep_Back();
    servo0 = RIGHT;
    off(DIRECTION);
    motor(MOTOR, speed);
    lastmove = 0;
    isstopped = 0;
    return 0;
}

int left(int speed){
    
    //printf("left\n");
    Prep_Forward();
    servo0 = LEFT;
    fd(DIRECTION);
    motor(MOTOR, speed);
    lastmove = 1; 
    isstopped = 0;
    return 1;
}

int rev_left(int speed){
    
    //printf("Reverse Left\n");
    Prep_Back();
    servo0 = LEFT;
    off(DIRECTION);
    motor(MOTOR, speed);
    lastmove = 0;
    isstopped = 0;
    return 0;
}

int stop(){
    
    //printf("stop\n");
    servo0 = CENTER;
    off(1);
    isstopped = 1;
    Reset_Timer(0); // timer used to ensure motors dont switch directions with out a 4.3 seconds pause
    return 0;
}

void movement_init(){
    init_expbd_servos(1);
    
}



void Prep_Forward()
{
    if(lastmove != 1)
      {
        
        if(isstopped != 1) stop();
        while(Get_Timer(0) < 4300L); // ensure motors have been stopped for 4.3 seconds since stop when changing direction
        lastmove = 1;
    }    
    isstopped = 0;
}

void Prep_Back()
{
    if(lastmove != 0)
      {
        
        if(isstopped != 1) stop();
        while(Get_Timer(0) < 4300L); // ensure motors have been stopped for 4.3 seconds since stop when changing direction
        lastmove = 0;
    }    
    isstopped = 0;
}