#include "Main.h"

int KnockedBinders = 0;
int BinderThreshold = 42;

unsigned char UltrasonicA = 1, UltrasonicB = 9, BumperSwitch = 2;

void CMain ( void )
{
    StartUltrasonic ( UltrasonicA , UltrasonicB ) ;

    //while(1) {
    //    PrintToScreen("ULTRA: %d\n", GetUltrasonicCm ( UltrasonicA , UltrasonicB ));
    //}

    while(KnockedBinders < 4) {
        SetMotor ( 1 , -35 ) ;
        SetMotor ( 10 , -35 ) ;
        if(GetUltrasonicCm ( UltrasonicA , UltrasonicB ) < BinderThreshold) {
            Wait(500);
            SetMotor ( 1 , 35 ) ;
            SetMotor ( 10 , -35 ) ;
            while(GetDigitalInput ( BumperSwitch )) { }
            Wait(100);
            SetMotor ( 1 , -35 ) ;
            SetMotor ( 10 , 35 ) ;
            Wait(1000);
            KnockedBinders++;
        }
    }
    SetMotor(1, 0);
    SetMotor(10, 0);
}