/* ===========================================================================

   Author:  Bryan Shaw
   Date Started: 1-6-05
   Date Completed:
   Date last modified: 1-20-05
   Credit given to Bruce A. Furguson for beginning the code
   Rights given to ECE department of Rose-Hulman Inst. of Technology and
          Voltmerguson Enterprises
          
   Purpose:   This is the program that will be run on the goalie 'robot' for
   ECE361 winter quarter of the 2004-2005 school year.  The goalie will be a
   robot mounted on a track above the goal on the board.  It will move back and
   forth along the track with semi-randomness to its movements.  It will
   constantly broadcast a 'preamble' with it's IR link consisting of 1-2-1-2.
   It will then broadcast a 6-digit code.  If it receives this code in reverse
   it will move the goalie to one side of the goal and sit there for 5 seconds,
   then repeat.

============================================================================*/


#define percentReverse 20
#define lowRand 3
#define highRand 252 //Highest random number MINUS LOWRAND
#define sendDelay 5  //Delay after sending a number, in milliseconds


int receivedPre, receivedCode, bitCheck, ranTime, dir, ranDir;
int but1Pressed, but3Pressed, lastButPressed, retreating, num[6];


task main()
{

 SetTxPower(TX_POWER_HI);
 retreating = 0;               // Initialize all variables before loop begins
 but1Pressed = 0;
 but3Pressed = 0;
 SetOutput(OUT_A, OUT_ON);     // Start motor moving so that buttons doesn't
 dir = 1;
 start buttons;                // Start routine initially              have to
 start randomizer;             // Start routine initially

 while(1){                     // Main loop that runs constantly
//    PlaySound(SOUND_LOW_BEEP);
//    SetUserDisplay(value, position);

    receivedPre = 0;
    receivedCode = 0;
    bitCheck = 0;

    num[0]=Random(highRand)+lowRand;   // generate random code sequence
    num[1]=Random(highRand)+lowRand;   // digits vary from 3-255
    num[2]=Random(highRand)+lowRand;
    num[3]=Random(highRand)+lowRand;
    num[4]=Random(highRand)+lowRand;
    num[5]=Random(highRand)+lowRand;

    sendPreamble();                // Send the preamble to player's robot

    sendCode();                    // Send the code to player's robot

    ClearTimer(0);
    while (!receivedPre && (Timer(0) <= 20)) // Wait until preamble is received
    {                                        // back or 2 seconds has elapsed
        if (bitCheck == 0){            // If the first digit is being detected
            if (Message() == 1){       // is the current digit the first digit
                bitCheck++;            // If so, star checking the next digit
                ClearMessage();        // Clear the message buffer to detect
            }                                          // next digit
        }
        else if (bitCheck == 1){       //  Do this again for the net digit
             if(Message() == 2){
                 bitCheck++;
                 ClearMessage();
             }
        }
        else if (bitCheck == 2){       // Have to do it this way, because this
            if(Message() == 1){        // program will not be in synch with the
                bitCheck++;            // player's robot so we need to keep
                ClearMessage();        // checking for the same digit till its
            }                          // received, but also have to have the
        }                              // main while loop checking the timer,
        else if (bitCheck == 3){       // so we make if statements that
             if(Message() == 2){       // constantly cycle through.
                 bitCheck++;
                 ClearMessage();
                 receivedPre = 1;
             }
        }
    }
    bitCheck = 0;

    while (!receivedCode && (Timer(0) <= 20))      // This uses the same methods
    {                                              // as the preamble detector
        if (bitCheck == 0){                        // but detects the 6-digit
            if (Message() == num[5]){              // code in reverse
                bitCheck++;
                ClearMessage();
            }
        }
        else if (bitCheck == 1){
             if(Message() == num[4]){
                 bitCheck++;
                 ClearMessage();
             }
        }
        else if (bitCheck == 2){
            if(Message() == num[3]){
                bitCheck++;
                ClearMessage();
            }
        }
        else if (bitCheck == 3){
             if(Message() == num[2]){
                 bitCheck++;
                 ClearMessage();
             }
        }
        else if (bitCheck == 4){
             if(Message() == num[1]){
                 bitCheck++;
                 ClearMessage();
             }
        }
        else if (bitCheck == 5){
             if(Message() == num[0]){
                 bitCheck++;
                 ClearMessage();
                 receivedCode = 1;
             }
        }
    }
    bitCheck = 0;

    if (receivedCode){
        start retreat;       // Start the retreating routine
        retreating = 1;      // Set this now so the next line will see it
        while(retreating){}  // The retreat task will set this to 0 when done
    }
 }
}


/* This task will be used to move the robot back and forth semi-randomly
   and move in the opposite direction whenever it hits one of the endpoint
   buttons. */

task buttons()
{

 while(true){

    if(SENSOR_1 < 500 && !but1Pressed){   // If goalie hits this end, go back
        SetDirection(OUT_A, OUT_REV);
        dir = 1;
        but1Pressed = 1;
        lastButPressed = 1;
    }
    else if(SENSOR_1 >= 500) but1Pressed = 0;  // Set this only after button not
                                               // pressed to prevent constant
    if(SENSOR_3 < 500 && !but3Pressed){        // direction setting when pressed
        SetDirection(OUT_A, OUT_FWD);          // Same ideas here
        dir = 0;
        but3Pressed = 1;
        lastButPressed = 0;
    }
    else if(SENSOR_3 >= 500) but3Pressed = 0;
    
 }
}


/* This task will run constantly except when retreating.  It will simply toggle
   the direction of the robot at random intervals between 0.2 and 1 second.
   It does not set the direction to anything specific, so the button routine
   will still ensure that it stays on the track and does not attempt to knock
   the buttons off. */

task randomizer()
{
 while(1)
 {
    ranDir = Random(100);
    if(ranDir >= percentReverse)
    {
        if(lastButPressed == 1)
            SetDirection(OUT_A,OUT_REV);
        if(lastButPressed == 0)
            SetDirection(OUT_A,OUT_FWD);
    }
    else
    {
        if(lastButPressed == 1)
            SetDirection(OUT_A,OUT_FWD);
        if(lastButPressed == 0)
            SetDirection(OUT_A,OUT_REV);
    }

    ranTime = Random(8)+2;    // Min of 2, Max of 10, 10 = 1 sec
    ClearTimer(1);
    while(Timer(1) <= ranTime){}
 }
}


/* This is the task that will be used to retreat the goalie when the proper
   set of signals is received.  It will retreat to the proper side and stay there
   for 5 seconds. */

task retreat()
{
 stop buttons;          // Stop motor from going back and forth due to this task
 stop randomizer;

 retreating = 1;

 SetDirection(OUT_A, OUT_REV);       // Send goalie twards retreat side
 while(SENSOR_3 >= 500){}            // Wait till it gets there
 SetOutput(OUT_A, OUT_OFF);          // Shut down the motor
 
 ClearTimer(0);                      // Wait for 5 seconds
 while(Timer(0) <= 50){}

 SetDirection(OUT_A, OUT_FWD);       // Start the motor heading away from button
 SetOutput(OUT_A, OUT_ON);

 Wait(10);                           // Wait 1/10 second to ensure motor clears
                                     // the button before 'buttons' is restarted
 retreating = 0;

 start buttons;                      // Resume normal operation
 start randomizer;
 stop retreat;                       // Stop retreat task
}


/* These are the two functions that are called in order to send out the
   preamble and the main code when needed*/
   
void sendPreamble(void)
{
 SendMessage(1); Wait(sendDelay);     // Send a 1-2-1-2 sequence for preamble
 SendMessage(2); Wait(sendDelay);     // then give player robot time to store data
 SendMessage(1); Wait(sendDelay);     // Repeat
 SendMessage(2); Wait(sendDelay);
}

void sendCode(void)
{
    SendMessage(num[0]); Wait(sendDelay);       // Send first digit
    SendMessage(num[1]); Wait(sendDelay);       // Send second
    SendMessage(num[2]); Wait(sendDelay);       // Etc...
    SendMessage(num[3]); Wait(sendDelay);
    SendMessage(num[4]); Wait(sendDelay);
    SendMessage(num[5]); Wait(sendDelay);
}