/******************************************/
/*  Lab 1 Program                         */
/*  Tom Covos                             */
/*  Simon Foucher 260 223 197             */
/*  Sept 14th/2007                        */
/*                                        */
/******************************************/


/*********************************/
/*  Functions Declarations       */
/*********************************/
task locomotion();
task display();


/*********************************/
/*  Variables Declarations       */
/*********************************/
bool go = 0;


/************************/
/*  Threads             */
/************************/


task main() {

/*****************************/
/*  Innitialising sensors    */
/*  and threads              */
/*****************************/

    SetSensor(S1,SENSOR_LIGHT);  // Configure light sensor
    SetSensorLowspeed(S2);       // Configure ultrasonic sensor

    start display;
    start locomotion;



/*****************************/
/*  Looking for user input   */
/*****************************/

    while (true) {
        if (ButtonCount(BTNLEFT,true)) // if left button has been pressed
            go = true;
        if (ButtonCount(BTNRIGHT,true)) // if right button has been pressed
            go = false;
        Wait(10); // 10ms delay: we don't need more than 10ms response time
    }
}




task locomotion() {
/******************************************/
/*  the locomotion pattern is broken up   */
/*  into short-time-interval (20ms) steps */
/*  so that movement can be paused by     */
/*  waiting between steps.                */
/******************************************/


    for (int step = 0; true; step = (step+1)%80 ) {
        if (!go) {
/******************************************/
/*  If we're in pause state, stop both    */
/*  motors, and then wait until we go     */
/*  out of pause mode                     */
/******************************************/
            OffEx(OUT_BC,RESET_NONE);
            do Wait(20) while (!go);
        }
        
/*******************************************/
/* Movment:                                */
/*                                         */
/* First 60 steps (1.2 s each): have the   */
/* tribot move forward                     */
/* Remaining 40 steps (0.6s each): Tribot  */
/* rotates in place                        */
/*******************************************/
        if (step<62)
            OnFwdEx(OUT_BC,80,RESET_NONE);
        else {
            OnFwdEx(OUT_B,80,RESET_NONE);
            OnRevEx(OUT_C,80,RESET_NONE);
        }
        Wait(20);
    }
}


task display() {
    int temp;
    while (true) {
        ClearScreen();

        TextOut(0,LCD_LINE1,"Motor A (R):");
        NumOut(80,LCD_LINE1,GetOutput(OUT_B, TachoCount));

        TextOut(0,LCD_LINE2,"Motor B (L):");
        NumOut(80,LCD_LINE2,GetOutput(OUT_C, TachoCount));

        TextOut(0,LCD_LINE4,"Light S1:");
        NumOut(60,LCD_LINE4,Sensor(S1));

        TextOut(0,LCD_LINE5,"Sonic S2:");
        NumOut(60,LCD_LINE5,temp);

        if (!go)
            TextOut(0,LCD_LINE8,"[paused]");
        
        temp = SensorUS(S2);

        /*******************************************/
        /* We take US reading once all other       */
        /* display is done because the I2C read    */
        /* takes a lot of time                     */
        /*******************************************/

        Wait(100);
    }
}



