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

Dead Reckoning Navigation
Karan M. Gupta (karanmg@gmail.com)
*************************************************************/

#define RIGHT 0
#define LEFT 1

#define A_FWD_PWR 2
#define C_FWD_PWR 2
#define A_TURN_PWR 5
#define C_TURN_PWR 5

#define CLK_ONE_FT 4
#define CLK_L_NINETY 1
#define CLK_R_NINETY 1

#define MY_EVENT1 0
#define MY_EVENT2 1

int counter1 = 0;
int counter2 = 0;

task main()
{
     setup();
     go_fwd(1);
     turn_ninety(RIGHT);
     /*go_fwd(2);
     turn_ninety(RIGHT);
     go_fwd(5);
     turn_ninety(RIGHT);
     go_fwd(4);
     turn_ninety(LEFT);
     go_fwd(5);
     turn_ninety(LEFT);
     go_fwd(2);
     turn_ninety(LEFT);*/
}

void setup()
{
  SetSensorType(SENSOR_1, SENSOR_TYPE_TOUCH);
  SetSensorType(SENSOR_3, SENSOR_TYPE_TOUCH);
  SetSensorMode(SENSOR_1, SENSOR_MODE_RAW);
  SetSensorMode(SENSOR_3, SENSOR_MODE_RAW);

  SetEvent(MY_EVENT1, SENSOR_1, EVENT_TYPE_PRESSED);
  SetEvent(MY_EVENT2, SENSOR_3, EVENT_TYPE_PRESSED);
}

void go_fwd(int distance)
{
     SetPower(OUT_A, A_FWD_PWR);
     SetPower(OUT_C, C_FWD_PWR);
     Fwd(OUT_C);
     Fwd(OUT_A);
     counter1 = 0;
     On(OUT_A + OUT_C);

     while(true && counter1 <= (distance * CLK_ONE_FT))
	   {
                monitor(EVENT_MASK(MY_EVENT1))
                {
                until(false);
	            }
                catch
		       {
                        counter1++;
		       }
	   }

     Off(OUT_A + OUT_C);
}

void turn_ninety(int direction)
{
     counter1 = 0;
     counter2 = 0;
     if(direction == RIGHT)
     {
         Rev(OUT_A);
         Fwd(OUT_C);
     }
     else
     {
         Fwd(OUT_C);
         Rev(OUT_A);
     }
     SetPower(OUT_A, A_TURN_PWR);
     SetPower(OUT_C, C_TURN_PWR);
     On(OUT_A + OUT_C);
     if(direction == LEFT)
     {
              while(true && (counter1 <= CLK_L_NINETY && counter2 <= CLK_R_NINETY))
	            {
                         monitor(EVENT_MASK(MY_EVENT1))
                         {
                         }
		                     catch
		                     {
                              counter1++;
                         }
		                     monitor(EVENT_MASK(MY_EVENT2))
                         {
	                       }
		                     catch
		                     {
                              counter2++;
                         }
              }
     }
     else
     {
              while(true && (counter2 <= CLK_L_NINETY && counter1 <= CLK_R_NINETY))
	            {
                         monitor(EVENT_MASK(MY_EVENT1))
                         {
                         }
		                     catch
		                     {
                              counter1++;
                         }

		                     monitor(EVENT_MASK(MY_EVENT2))
                         {
	                       }
		                     catch
		                     {
                              counter2++;
                         }
              }
      }
     Off(OUT_A + OUT_C);
}