#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(); }