/*************************************************************************
**																		**
** Main: Simple bumper car												**
**									                                    **
*************************************************************************/

#include "legocore.h"

#define LEFT_MOTOR		OUT_CHAN0
#define RIGHT_MOTOR		OUT_CHAN1

#define LEFT_BUMPER		IN_CHAN1
#define RIGHT_BUMPER	IN_CHAN0

WaitSecond(unsigned char Num)
{
    unsigned char i;

    for(i=0;i<Num;i++){
	    Waitms(1000);
    }
}

main()
{
    unsigned char Speed = 128;
    unsigned int HitFree = 0;
    int i;

    LegoInit();

    SetOutputValue(OUT_ALL_CHAN, 0);
    SetForward(LEFT_MOTOR | RIGHT_MOTOR);
    SetOutputValue(LEFT_MOTOR|RIGHT_MOTOR,Speed);
    Waitms(200);

    while(1){
	    if(GetInput(RIGHT_BUMPER) < 128){  /* Button pressed */
	        Speed = 128;
	        SetBrake(LEFT_MOTOR|RIGHT_MOTOR);
	        WaitSecond(1);
	        SetReverse(LEFT_MOTOR|RIGHT_MOTOR);
	        SetOutputValue(LEFT_MOTOR|RIGHT_MOTOR,Speed);
	        Waitms(500);
	        SetForward(RIGHT_MOTOR);    // Turn Left
	        Waitms(500);
	        SetFloat(LEFT_MOTOR|RIGHT_MOTOR);
	        Waitms(250);
	        SetForward(LEFT_MOTOR);
	        SetOutputValue(LEFT_MOTOR|RIGHT_MOTOR,Speed);
	        HitFree = 0;
	    }
	    if(GetInput(LEFT_BUMPER) < 128){ /* Button pressed */
	        Speed = 128;
	        SetBrake(LEFT_MOTOR|RIGHT_MOTOR);
	        WaitSecond(1);
	        SetReverse(LEFT_MOTOR|RIGHT_MOTOR);
	        SetOutputValue(LEFT_MOTOR|RIGHT_MOTOR,Speed);
	        Waitms(750);
	        SetForward(LEFT_MOTOR);    // Turn Right
	        Waitms(1250);
	        SetFloat(LEFT_MOTOR|RIGHT_MOTOR);
	        Waitms(150);
	        SetForward(RIGHT_MOTOR);
	        SetOutputValue(LEFT_MOTOR|RIGHT_MOTOR,Speed);
	        HitFree = 0;
	    }
	    Waitms(50);
	    HitFree++;
	    if(HitFree>100){  // 5 seconds
	        Speed = 255;
	        SetOutputValue(LEFT_MOTOR|RIGHT_MOTOR,Speed);
	    }
        if(HitFree>200){ // 10 seconds
	        SetFloat(LEFT_MOTOR|RIGHT_MOTOR);
	        WaitSecond(3);
            SetReverse(RIGHT_MOTOR);
            SetForward(LEFT_MOTOR);    // Turn Right
            Speed = 10;
            for(i=0;i<50;i++){
	            SetOutputValue(LEFT_MOTOR|RIGHT_MOTOR,Speed);
	            Waitms(80);
                if(Speed<230)
                    Speed += 10;
                else
                    Speed = 255;
            }
	        SetFloat(LEFT_MOTOR|RIGHT_MOTOR);
	        Waitms(1500);
	        SetForward(RIGHT_MOTOR);
            Speed = 128;
	        SetOutputValue(LEFT_MOTOR|RIGHT_MOTOR,Speed);
	        HitFree = 0;
        }
    }
}