/************************************************************
AI Robotics
Fall 2003

Time Based Robot Navigation
Karan M. Gupta (karanmg@gmail.com)
*************************************************************/

#define RIGHT 0
#define LEFT 1

#define AFWD_PWR 2
#define CFWD_PWR 2
#define OUTER_WHEEL_TURN_PWR 7
#define INNER_WHEEL_TURN_PWR 4

#define TIME_ONE_FT 140  // can be 125
#define TURN_TIME 43

#define WAIT_TIME 200

task main()
{
     go_fwd(10);
     turn_ninety(RIGHT, 3);
     go_fwd(2);
     turn_ninety(RIGHT, 3);     // can be 2
     go_fwd(5);
     turn_ninety(RIGHT, 0);
     go_fwd(4);
     turn_ninety(LEFT, 0);
     go_fwd(5);
     turn_ninety(LEFT, 0);
     go_fwd(2);
     turn_ninety(LEFT, 0);
}

void go_fwd(int distance)
{
     SetPower(OUT_A, AFWD_PWR);
     SetPower(OUT_C, CFWD_PWR);
     Fwd(OUT_C);
     Fwd(OUT_A);
     OnFor(OUT_A + OUT_C, distance * TIME_ONE_FT);
     Off(OUT_A + OUT_C);
     Wait(WAIT_TIME);
}

void turn_ninety(int direction, int add)
{
     if(direction == RIGHT)
     {
         Fwd(OUT_C);
         Rev(OUT_A);
         SetPower(OUT_A, INNER_WHEEL_TURN_PWR);
         SetPower(OUT_C, OUTER_WHEEL_TURN_PWR);
     }
     else
     {
         Fwd(OUT_A);
         Rev(OUT_C);
         // NOTE: Powers are OPPOSITE!!
         SetPower(OUT_C, INNER_WHEEL_TURN_PWR);
         SetPower(OUT_A, OUTER_WHEEL_TURN_PWR);
     }
     OnFor(OUT_A + OUT_C, TURN_TIME + add);
     Off(OUT_A + OUT_C);
     Wait(WAIT_TIME);
}