#define digital_15 15 
#define digital_14 14 
#define digital_13 13 
#define digital_12 12 
#define analog_16 16 
#define ENCODER_PORT 7 
#define motor_3 3 



int x; 



/* 
void    beacontest() {
 set_digital_out(8);
 clear_digital_out(8);
 ir_transmit_on();


 while(stop_button()    == 0){
 if (digital(digital_15) == 1)
 printf("15 North working\n");
 if (digital(digital_14)    == 1)
 printf("14 West working\n");
 if (digital(digital_13) == 1)
 printf("13    South working\n");
 if (digital(digital_12) == 1)
 printf("12 East working\n");
    }} 
*/ 


void forward(int dist, int speed) {
 reset_encoder(ENCODER_PORT);
 motor(3, speed); 
while (read_encoder(ENCODER_PORT)    < dist) {
sleep(0.1); } 
motor(3,0);
 }


 int getbeaconval(){
 /* set_digital_out(8);    
clear_digital_out(8); 
ir_transmit_on(); 
*/ 
if (digital(digital_15) == 1) return    1; 
if (digital(digital_14) == 1) return 2; 
if (digital(digital_13) == 1) return    3; 
if (digital(digital_12) == 1) return 4; 
} 


void platformturn(){ 
int boolean;    
int tempbeacon; 
tempbeacon = 0; 
boolean = 0; 
while (boolean == 0) { 
tempbeacon    = getbeaconval(); 
if (tempbeacon == 1) 
{printf("North"); 
drive_car(); 
boolean    = 1;} 
if (tempbeacon == 2) 
{printf("West"); 
forward(74, 100); 
drive_car(); 
boolean    = 2;} 
if (tempbeacon == 3) 
{printf("South"); 
forward(154, 100); 
drive_car();    
boolean = 3;} 
if (tempbeacon == 4) 
{printf("East"); 
forward(73, -100); 
drive_car();    
boolean = 4;} 
} } 


void drive(int speed, float time){ 
motor(1, speed); 
motor(2,    speed); 
sleep(time); 
motor(1,0); 
motor(2,0);} 


#define NUM_FUNCS 6 


void go()    { 
int func = -1; 
int knobSample; 
ir_transmit_off(); 
while (!stop_button()) {
    knobSample = knob(); 
func = knobSample / (256 / NUM_FUNCS);
 
if (func == 0) 
printf("Encoder    = %d\n", read_encoder(ENCODER_PORT)); 
if (func == 1) printf("Platform\n"); 
if    (func == 2) printf("forward\n"); 
if (func == 3) printf("east\n"); 
if (func ==    4) printf("competition\n"); 
if (func == 5) printf("condor\n"); 

if (start_button())    { 
while(start_button()){} 
if (func == 1) platformturn(); 
if (func == 2) drive(100,    0.5); 
if (func == 3) forward(80, -100); 
if (func == 4) final(); 
if (func ==    5) operationCondor(); 
} 
sleep(0.1); 
} } 


void setup() { 
enable_encoder(ENCODER_PORT);    
set_digital_out(8); 
clear_digital_out(8); 
ir_transmit_off(); } 


void platform(){    
start_machine(6); 
platformturn(); } 


void final(){ 
start_machine(6); 
platformturn();    
printf("sleep 2"); 
/* drive(100, 0.85); 
servocontrol(); 
sleep(0.1); 
printf("sleep    3"); 
drive(100, 0.8); 
*/ 
} 


void servocontrol(){ 
enable_servos(); 
servo(1,4000);    
sleep(2.0); 
servo(1,1500); 
sleep(2.0); 
disable_servos();
 } 


void operationCondor(){    
motor(4,70); 
sleep(3.0); 
motor(4,0); 
} 


void drive_car(){ 
sleep(0.1); 
drive(100,    0.75); 
servocontrol(); 
sleep(0.05); 
printf("sleep 3"); 
drive(100, 0.75); 
sleep(0.05);    
motor(4,70); 
sleep(3.0); 
motor(4,0); 
} 


void main(){ 
x=0; 
setup(); 
go(); 
}