persistent float best_thresh_front_left, best_thresh_rear_left, best_thresh_rear_right; persistent float gyro_bias = 0.0; persistent float acc_bias_forward = 0.0; persistent float acc_bias_right = 0.0; persistent uint8 orientation_code; persistent bool team_color; persistent uint8 our_rf_number; persistent int control_frequency; persistent int state_bs = BS_NOTHING; persistent int state_ball = BALL_NONE; persistent float position_x, position_y; persistent float dest_start_x, dest_start_y; persistent float clusters_x[total_clusters]; persistent float clusters_y[total_clusters]; persistent float white_scoring_area_x[scoring_areas_per_team]; persistent float white_scoring_area_y[scoring_areas_per_team]; persistent bool virgin_clusters[total_clusters]; /* TO-DO LIST: Add gyro scale factor + calibrate (turn robot 360deg) vote make mission planner avoid the center obstacle and opponent look at voting status New strategy: find closest "thing" and use it, i.e. voting slot or either team's scoring region. have at least 3 balls or so stuff tires? ball sorter can get stuck in a state where there is a ball but the claw doesn't move Test accelerometers after bug fix & compare to shaf encoder Command servo castor and differential wheel drive in a consistent manner to prevent grinding/slipping. make alignment stencil reintroduce collision event code transfer align heading to velocity vector increase gyro integration frequency add a way to turn in reverse */ #define HUGE 1e37 #define true 1 #define false 0 #define BLACK 0 #define WHITE 1 #define pi (3.14159265358979) #define PI pi #define inches (100.0/12.0) #define feet (100.0) #define gravity (32.0 * feet) /*per second squared*/ #define DEGREES_TO_RADIANS (pi/180.0) #define RADIANS_TO_DEGREES (180.0/pi) bool start_or_stop_press() { bool result; while(1) { if (start_button()) { result = 1; break; } if (stop_button()) { result = 0; break; } } while(start_button() || stop_button()); beep(); return result; } /* OMG IC is retarded!!!! */ int float_to_int(float value) { if (value >= 32767.0) return 32767; else if (value > 0.0) return (int)value; else if (value > -0.5) return 0; else if (value > -1.0) return -1; else if (value > -32768.0) return (int)value; else return -32768; } float sign(float x) { if (x > 0.0) return 1.0; else if (x < 0.0) return -1.0; else return 0.0; } float fabs(float x) {if (x<0.0) return -x; else return x;} float atan2(float dy, float dx) { float angle; if (not_approx_zero(dx)) { if (dx < 0.0) return atan(dy/dx) + pi; else if (dy < 0.0) return atan(dy/dx) + 2.0*pi; else return atan(dy/dx); } else { if (dy < 0.0) return 3.0 * pi / 2.0; else if (dy==0.0) return 0.0; else return pi / 2.0; } } float recirculate_angle(float a) { if (a >= pi) return a - 2.0*pi; else if (a < -pi) return a + 2.0*pi; else return a; } void clear_lcd() {printf("\n"); printf("");} long long_of_int(int x) {if(x>=0) return (long)x; else return (long)x - 65536L;} void printfloat5(float value) { int num; num = float_to_int(10.0 * value); if (num < 10) printf(" %d ", num); else if (num < 100) printf(" %d ", num); else if (num < 1000) printf(" %d ", num); else printf("%d ", num); } void printint3(int num) { if (num < 10) printf(" %d ", num); else if (num < 100) printf(" %d ", num); else printf("%d ", num); } void printint2(int num) { if (num < 10) printf(" %d ", num); else printf("%d ", num); } void beep56() {/*beep();*/} int analog_discard(int port) { analog(port); return analog(port); } int safe_analog(int port) { if (port >= 16) analog(port); return analog(port); } float noisy_analog(int port) { return (float)(safe_analog(port) + analog(port) + analog(port) + analog(port)) / 4.0; } float squared(float x) {return x*x;} float twice(float x) {return x+x;} float distance(float x1, float y1, float x2, float y2) { return sqrt(squared(x2-x1)+squared(y2-y1)); } float dot_product(float x1, float y1, float x2, float y2) { return x1*y1 + x2*y2; } bool not_approx_zero(float denominator) { /*return (fabs(denominator) > 1e-6);*/ return (fabs(denominator) > 1e-3); } float dist_point_line (float xp, float yp, float xl1, float yl1, float xl2, float yl2) { float a, b, denominator; a = yl2 - yl1; b = xl1 - xl2; denominator = sqrt(a * a + b * b); if (not_approx_zero(denominator)) return (a * (xp - xl1) + b * (yp - yl1)) / denominator; else return 0.0; } /* Analog sensor ports: */ #define gyro_port 3 #define acc_forward_port 4 #define acc_right_port 5 #define SENSOR_BALL 6 #define CASTOR_SHAFT_ENCODER_PORT 7 #define reflect_front_left_port 16 #define reflect_rear_left_port 17 #define reflect_rear_right_port 18 /* Motor ports: */ #define left_motor_port_a 0 #define right_motor_port_a 1 #define left_motor_port_b 2 #define right_motor_port_b 3 #define muncher_motor_port_a 4 #define muncher_motor_port_b 5 float castor_angle = 0.0; void set_castor_angle(float angle) /* radians clockwise from forward vector */ { int number; castor_angle = angle; /* remember it */ servo(SERVO_CASTOR, CASTOR_STRAIGHT + float_to_int(angle/(pi/2.0) * (float)SERVO_90_DEGREES)); } long stopped_since; void detect_drive_collision() { if (fabs(rate_forward) < 1.0*inches /*per second*/) stopped_since = mseconds(); /*if (mseconds() - stopped_since > 500L) navigate_state = BACK_UP;*/ } /* Servo ports: */ #define SERVO_CASTOR 0 #define SERVO_CLAW 1 #define SERVO_BALL_GATE 2 /* The units of theta are converter LSBs * ms 1 deg/s = 5mV = 0.256 LSB for ADXRS300, 12.5mV = 0.64LSB for ADXRS150 1 deg = 0.256LSB * 1000ms = 256 (stored in lsb_ms_per_deg) To convert theta to degrees, divide by lsb_ms_per_deg. */ #define raw_gyro_to_rps (1.0/0.256 * DEGREES_TO_RADIANS) #define gyro_calibrated_scale_factor (2.0*pi/(2.0*pi+0.675)) #define raw_acc_to_cfpss (gravity/68.5) /* #define MOTOR_DRIVER_VOLTAGE_SUPPLY 12.0 /*dual Hawkers */ #define MOTOR_DRIVER_VOLTAGE_SUPPLY 6.0 /* single Hawkers */ /*#define MOTOR_DRIVER_VOLTAGE_SUPPLY 10.0*/ /* Handyboard Only */ #define MOTOR_COMMAND_PEAK 100.0 #define MOTOR_IMPEDANCE 2.0 /* #define MOTOR_DRIVER_MAX_WATTS 2.075 #define SAFE_MOTOR_MAXIMUM (MOTOR_IMPEDANCE / (MOTOR_DRIVER_VOLTAGE_SUPPLY*MOTOR_DRIVER_VOLTAGE_SUPPLY) * MOTOR_DRIVER_MAX_WATTS * MOTOR_COMMAND_PEAK) */ #define MOTOR_DRIVER_MAX_CURRENT 2.0 /* Amps */ #define SAFE_MOTOR_MAXIMUM ((MOTOR_DRIVER_VOLTAGE_SUPPLY / MOTOR_IMPEDANCE) / MOTOR_DRIVER_MAX_CURRENT * MOTOR_COMMAND_PEAK) #define OPEN_REAR_GATE_LEFT 3800 #define OPEN_REAR_GATE_RIGHT 200 #define CLOSE_REAR_GATE 2100 #define ROBOT_RADIUS (9.0*inches) /*#define total_clusters 12*/ #define total_clusters 10 #define bool int #define uint8 int #define reflectance_points 4 long sum_front_left, sum_rear_left, sum_rear_right; long sum_count; void calibrate_reflectance_sensors() { uint8 m,n; long tstart; float diff; float mean_front_left[reflectance_points], mean_rear_left[reflectance_points], mean_rear_right[reflectance_points]; float last_mean_front_left, last_mean_rear_left, last_mean_rear_right; float best_diff_front_left, best_diff_rear_left, best_diff_rear_right; int raw_front_left, raw_rear_left, raw_rear_right; for(n=0; n<reflectance_points; n++) { clear_lcd(); printf("Orient %d, then press start.", n); sum_front_left = 0L; sum_rear_left = 0L; sum_rear_right = 0L; sum_count = 0L; /*start_press();*/ if (start_or_stop_press()==0) { clear_lcd(); return; } tstart = mseconds(); while(mseconds()-tstart < 500L) { raw_front_left = safe_analog(reflect_front_left_port); raw_rear_left = safe_analog(reflect_rear_left_port); raw_rear_right = safe_analog(reflect_rear_right_port); sum_front_left += long_of_int(raw_front_left); sum_rear_left += long_of_int(raw_rear_left); sum_rear_right += long_of_int(raw_rear_right); sum_count++; mean_front_left[n] = (float)sum_front_left / (float)sum_count; mean_rear_left[n] = (float)sum_rear_left / (float)sum_count; mean_rear_right[n] = (float)sum_rear_right / (float)sum_count; clear_lcd(); printint3(raw_rear_right); printint3(raw_rear_left); printint3(raw_front_left); printf(" "); printfloat5(mean_rear_right[n]); printfloat5(mean_rear_left[n]); printfloat5(mean_front_left[n]); } } for (n=0; n<reflectance_points; n++) { for (m=0; m<reflectance_points; m++) { if (m != n) { diff = fabs(mean_front_left[m] - mean_front_left[n]); if (diff > best_diff_front_left) { best_diff_front_left = diff; best_thresh_front_left = (mean_front_left[m] + mean_front_left[n]) / 2.0; } diff = fabs(mean_rear_left[m] - mean_rear_left[n]); if (diff > best_diff_rear_left) { best_diff_rear_left = diff; best_thresh_rear_left = (mean_rear_left[m] + mean_rear_left[n]) / 2.0; } diff = fabs(mean_rear_right[m] - mean_rear_right[n]); if (diff > best_diff_rear_right) { best_diff_rear_right = diff; best_thresh_rear_right = (mean_rear_right[m] + mean_rear_right[n]) / 2.0; } } } } beep(); clear_lcd(); printint3(float_to_int(best_diff_rear_right)); printint3(float_to_int(best_diff_rear_left)); printint3(float_to_int(best_diff_front_left)); printf(" "); /*start_press();*/ } uint8 initial_side_table[8] = {BLACK,BLACK,BLACK,WHITE,BLACK,WHITE,WHITE,WHITE}; float dirt[8] = {180.0, -90.0, 0.0, 180.0, 90.0, 90.0, 0.0, -90.0}; /*uint8 initial_target_cluster[8] = {10,0,2,7,4,9,11,5};*/ uint8 initial_target_cluster[8] = {3,0,2,7,4,9,8,5}; char initial_orientation[] = "SWNSEENW"; void sum_reflectance_reads() { int raw_front_left, raw_rear_left, raw_rear_right; raw_front_left = safe_analog(reflect_front_left_port); raw_rear_left = safe_analog(reflect_rear_left_port); raw_rear_right = safe_analog(reflect_rear_right_port); sum_front_left += long_of_int(raw_front_left); sum_rear_left += long_of_int(raw_rear_left); sum_rear_right += long_of_int(raw_rear_right); } long sum_gyro, sum_acc_forward, sum_acc_right; void start_calibration() { sum_gyro = 0L; sum_acc_forward = 0L; sum_acc_right = 0L; sum_front_left = 0L; sum_rear_left = 0L; sum_rear_right = 0L; sum_count = 0L; } void sum_analog_reads() { sum_gyro += long_of_int(analog(gyro_port)); sum_acc_forward += long_of_int(analog(acc_forward_port)); sum_acc_right += long_of_int(analog(acc_right_port)); } void calibrate() { /* Robot must be stationary during calibration */ sum_analog_reads(); sum_reflectance_reads(); sum_count++; } float gyro_rate; float acc_forward, acc_right; float rate_forward, rate_right; float heading; void finish_calibration() { int bit2, bit1, bit0; float fsum_count; float mean_front_left, mean_rear_left, mean_rear_right; fsum_count = (float)sum_count; if (fsum_count != 0.0) { gyro_bias = (float)sum_gyro / fsum_count; acc_bias_forward = (float)sum_acc_forward / fsum_count; acc_bias_right = (float)sum_acc_right / fsum_count; mean_front_left = (float)sum_front_left / fsum_count; mean_rear_left = (float)sum_rear_left / fsum_count; mean_rear_right = (float)sum_rear_right / fsum_count; } bit2 = (mean_rear_right < best_thresh_rear_right); bit1 = (mean_rear_left < best_thresh_rear_left); bit0 = (mean_front_left < best_thresh_front_left); orientation_code = bit2<<2 | bit1<<1 | bit0; heading = dirt[orientation_code] * DEGREES_TO_RADIANS; team_color = initial_side_table[orientation_code]; position_x = 0.0; if (team_color==WHITE) position_y = -ROBOT_START_Y; else position_y = ROBOT_START_Y; clear_lcd(); printf("%d%d%d ", bit2, bit1, bit0); if (team_color) printf("WHITE"); else printf("BLACK"); printf(" TEAM "); printf("Heading: %d", float_to_int(heading * RADIANS_TO_DEGREES)); } void start_machine56() { long start_time; printf("Listening for RF...\n"); while (rf_match_running == 1) calibrate(); printf("RF is working!\n"); start_time = mseconds(); while (mseconds() - start_time < 1000L) calibrate(); printf("Waiting for ON. STOP to bypass.\n"); while (rf_match_running == 0) { calibrate(); if (stop_button()) { while(stop_button()); break; } } reset_system_time(); } float vector_to_heading(float x, float y) { return recirculate_angle(pi/2.0 - atan2(y,x)); } /* A plan consists of the following (a line to navigate to and traverse) */ float dest_end_x, dest_end_y; float score_x, score_y; uint8 nearest_cluster; bool driving_forward = true; void drive(float heading_error, bool forward) { float power; driving_forward = forward; if (forward) power = DRIVE_POWER; else power = -DRIVE_POWER; set_castor_angle(-HEADING_ERROR_GAIN * heading_error); motor(left_motor_port_a, float_to_int(power)); motor(left_motor_port_b, float_to_int(power)); motor(right_motor_port_a, float_to_int(power)); motor(right_motor_port_b, float_to_int(power)); } void turn(float heading_error) { float turn_power; driving_forward = 0; turn_power = -heading_error * HEADING_GAIN; set_castor_angle(90.0 * DEGREES_TO_RADIANS); motor(left_motor_port_a, float_to_int(-turn_power)); motor(left_motor_port_b, float_to_int(-turn_power)); motor(right_motor_port_a, float_to_int(turn_power)); motor(right_motor_port_b, float_to_int(turn_power)); } /* Navigation states: */ #define TURN_TOWARDS_TARGET 0 #define DRIVE_TO_TARGET 1 #define TURN_INTO_TARGET 2 #define TRAVERSE_TARGET 3 #define DEPLOY1 5 #define DEPLOY2 6 #define BACK_UP 7 #define DRIVE_POWER 80.0 #define TRAVERSE_POWER (DRIVE_POWER/2.0) #define CASTOR_STRAIGHT 2000 #define CASTOR_TURN (3930-140) int navigate_state; #define CLOSE_ENOUGH (1.0*inches) #define ANGLE_CLOSE_ENOUGH (3.0*DEGREES_TO_RADIANS) #define HEADING_GAIN 100.0 /*#define HEADING_GAIN 10.0*/ #define HEADING_ERROR_GAIN 2.0 #define DEPLOY_DURATION 2000L bool deploying = false; float toggle = 1.0; long nav_state_timestamp; int previous_navigate_state; float desired_heading, heading_error; void navigate() { float command; long mseconds_in_state; if (navigate_state != previous_navigate_state) { nav_state_timestamp = mseconds(); previous_navigate_state = navigate_state; } mseconds_in_state = mseconds() - nav_state_timestamp; if (navigate_state==TURN_TOWARDS_TARGET) { desired_heading = vector_to_heading(dest_start_x - position_x, dest_start_y - position_y); heading_error = recirculate_angle(desired_heading - heading); turn(heading_error); if (fabs(heading_error)<ANGLE_CLOSE_ENOUGH && rf_update_count0>0 && rf_update_count1>0 || mseconds_in_state > 4000L) { beep56(); navigate_state = DRIVE_TO_TARGET; } } else if (navigate_state==DRIVE_TO_TARGET) { desired_heading = vector_to_heading(dest_start_x - position_x, dest_start_y - position_y); heading_error = recirculate_angle(desired_heading - heading); drive(heading_error, 1); detect_drive_collision(); if (fabs(recirculate_angle(desired_heading - heading)) > 330.0 * DEGREES_TO_RADIANS || mseconds_in_state > 15000L) { navigate_state = TURN_INTO_TARGET; beep56(); } } else if (navigate_state==TURN_INTO_TARGET) { desired_heading = vector_to_heading(dest_end_x - dest_start_x, dest_end_y - dest_start_y); heading_error = recirculate_angle(desired_heading - heading); turn(heading_error); if (fabs(heading_error)<ANGLE_CLOSE_ENOUGH || mseconds_in_state > 4000L) { if (deploying) { start_deploying(); beep56(); } else navigate_state = TRAVERSE_TARGET; beep56(); } } else if (navigate_state==TRAVERSE_TARGET) { desired_heading = vector_to_heading(dest_end_x - dest_start_x, dest_end_y - dest_start_y); heading_error = recirculate_angle(desired_heading - heading); drive(heading_error, 1); detect_drive_collision(); if (fabs(recirculate_angle(desired_heading - heading)) > 330.0 * DEGREES_TO_RADIANS || mseconds_in_state > 4000L) { virgin_clusters[nearest_cluster] = false; make_new_plan(); navigate_state = TURN_TOWARDS_TARGET; beep56(); } } else if (navigate_state==DEPLOY1) { set_castor_angle(0.0); servo(SERVO_BALL_GATE, OPEN_REAR_GATE_LEFT); turn(toggle * 90.0 * DEGREES_TO_RADIANS); balls_green = 0; if (mseconds_in_state > DEPLOY_DURATION) { /*navigate_state = DEPLOY2;*/ make_new_plan(); navigate_state = TURN_TOWARDS_TARGET; deploying = false; } } else if (navigate_state==DEPLOY2) { set_castor_angle(0.0); servo(SERVO_BALL_GATE, OPEN_REAR_GATE_RIGHT); turn(toggle * 90.0 * DEGREES_TO_RADIANS); balls_red = 0; if (mseconds_in_state > DEPLOY_DURATION) { make_new_plan(); navigate_state = TURN_TOWARDS_TARGET; beep56(); deploying = false; } } else if (navigate_state==BACK_UP) { drive(toggle * 30.0 * DEGREES_TO_RADIANS, 0); if (mseconds_in_state > 3000L) { virgin_clusters[nearest_cluster] = false; make_new_plan(); navigate_state = TURN_TOWARDS_TARGET; beep56(); } } if (mseconds()>40000L && !deploying) go_to_scoring_region_now(); if (mseconds() > 56000L && navigate_state != DEPLOY1 && navigate_state != DEPLOY2) start_deploying(); toggle = -toggle; } void start_deploying() { if (rf_vote_winner==0) /* red */ navigate_state = DEPLOY2; else /* green */ navigate_state = DEPLOY1; } void main() { ao(); enable_servos(); servo(SERVO_CLAW, CLAW_CENTER); /*servo(SERVO_CASTOR, CASTOR_TURN);*/ set_castor_angle(90.0*DEGREES_TO_RADIANS); servo(SERVO_BALL_GATE, CLOSE_REAR_GATE); /*disable_servos();*/ init_globals(); populate_clusters(); calibrate_reflectance_sensors(); /*clear_lcd();*/ printf("Check rear gate!"); start_press(); clear_lcd(); printf("START TO COMMIT"); start_press(); disable_servos(); start_calibration(); rf_team = 56; rf_enable(); start_machine56(); finish_calibration(); enable_encoder(CASTOR_SHAFT_ENCODER_PORT); reset_encoder(CASTOR_SHAFT_ENCODER_PORT); start_process(gyro_service_process()); /*start_process(compete(), 1);*/ compete(); shutdown(); } void go_to_scoring_region_now() { find_nearest_scoring_region(&dest_start_x, &dest_start_y); dest_end_x = dest_start_x - sign(dest_start_x) * 6.0 * inches; dest_end_y = dest_start_y - sign(dest_start_y) * 6.0 * inches; deploying = true; navigate_state = TURN_TOWARDS_TARGET; } #define table_width (6.0*feet) #define table_height (8.0*feet) #define cluster_side (7.0*inches) #define cluster_strip (cluster_side + 4.0*inches) #define cluster_x_outer (table_width/2.0 - 6.0*inches) #define cluster_x (cluster_x_outer - 1.0*feet) #define triple_cluster_y (table_height/2.0 - 18.0*inches) #define double_cluster_y (triple_cluster_y - 1.0*feet) #define scoring_area_y (table_height/2.0 - 6.0*inches); #define scoring_area_x (table_width/2.0 - 6.0*inches); #define scoring_areas_per_team 3 void populate_clusters() { /* mixed */ clusters_x[0] = 0.0; clusters_y[0] = triple_cluster_y; /* green */ clusters_x[1] = -cluster_x; clusters_y[1] = triple_cluster_y; /* red */ clusters_x[2] = cluster_x; clusters_y[2] = triple_cluster_y; /* green */ clusters_x[3] = -cluster_x_outer; clusters_y[3] = double_cluster_y; /* red */ clusters_x[4] = cluster_x_outer; clusters_y[4] = double_cluster_y; /* mixed */ clusters_x[5] = 0.0; clusters_y[5] = -triple_cluster_y; /* green */ clusters_x[6] = -cluster_x; clusters_y[6] = -triple_cluster_y; /* red */ clusters_x[7] = cluster_x; clusters_y[7] = -triple_cluster_y; /* green */ clusters_x[8] = -cluster_x_outer; clusters_y[8] = -double_cluster_y; /* red */ clusters_x[9] = cluster_x_outer; clusters_y[9] = -double_cluster_y; if (false) { /* mixed/barrier */ clusters_x[10] = 0.0; clusters_y[10] = cluster_side/2.0; /* mixed/barrier */ clusters_x[11] = 0.0; clusters_y[11] = -cluster_side/2.0; } /* negate y component for black scoring areas: */ white_scoring_area_x[0] = -scoring_area_x; white_scoring_area_y[0] = scoring_area_y; white_scoring_area_x[1] = 0.0; white_scoring_area_y[1] = -scoring_area_y; white_scoring_area_x[2] = scoring_area_x; white_scoring_area_y[2] = scoring_area_y; } float cos_heading, sin_heading; long gyro_service_time_last = 0L; long delta_ms; void gyro_service_process() { long time_now; float delta_time; for(;;) { time_now = mseconds(); /*delta_ms = time_now - gyro_service_time_last;*/ delta_time = (float)(time_now - gyro_service_time_last) / 1000.0; gyro_service_time_last = time_now; /*gyro_rate = (noisy_analog(gyro_port) - gyro_bias) * raw_gyro_to_rps;*/ gyro_rate = ((float)analog(gyro_port) - gyro_bias) * raw_gyro_to_rps * gyro_calibrated_scale_factor; heading = recirculate_angle(heading + gyro_rate * delta_time); defer(); } } void service_sensors() { /* gyro_rate = (noisy_analog(gyro_port) - gyro_bias) * raw_gyro_to_rps * gyro_calibrated_scale_factor; heading = recirculate_angle(heading + gyro_rate * dt); */ cos_heading = cos(heading); sin_heading = sin(heading); acc_forward = -(noisy_analog(acc_forward_port) - acc_bias_forward) * raw_acc_to_cfpss; acc_right = (noisy_analog(acc_right_port) - acc_bias_right) * raw_acc_to_cfpss; /* acc_forward = -((float)analog(acc_forward_port) - acc_bias_forward) * raw_acc_to_cfpss; acc_right = ((float)analog(acc_right_port) - acc_bias_right) * raw_acc_to_cfpss; */ rate_forward += acc_forward * dt; rate_right += acc_right * dt; if (false) { position_y += (rate_forward * cos_heading + rate_right * sin_heading) * dt; position_x += (rate_forward * sin_heading + rate_right * cos_heading) * dt; } } #define HOLES_IN_PULLY_WHEEL 6 #define WHEEL_DIAMETER (2.0*inches) #define WHEEL_CIRCUMFRENCE (WHEEL_DIAMETER * pi) #define CASTOR_SHAFT_CENTIFEET_PER_COUNT (WHEEL_CIRCUMFRENCE / (float)(2*HOLES_IN_PULLY_WHEEL)) #define SERVO_90_DEGREES 1790 #define CASTOR_SERVO_ZERO_HEADING 2000 void service_shaft_encoder() { float ds, theta, sine, cosine, dsdt; int counts; counts = read_encoder(CASTOR_SHAFT_ENCODER_PORT); reset_encoder(CASTOR_SHAFT_ENCODER_PORT); if (driving_forward) ds = (float)counts * CASTOR_SHAFT_CENTIFEET_PER_COUNT; else /* driving_backward */ ds = -(float)counts * CASTOR_SHAFT_CENTIFEET_PER_COUNT; /*if (not_approx_zero(dt))*/ dsdt = ds / dt; theta = (heading + castor_angle); /* sine = sin(theta); cosine = cos(theta); position_y += ds * cosine + ds * sine; position_x += ds * sine + ds * cosine; */ /* position_x += ds * cos(theta); position_y += ds * sin(theta); */ if (false) { position_y += ds * cos(theta); position_x += ds * sin(theta); } } int first_rf_x0, first_rf_y0; int first_rf_x1, first_rf_y1; int last_rf_x0, last_rf_y0; int last_rf_x1, last_rf_y1; uint8 rf_update_count0, rf_update_count1; float rf_vel_x0, rf_vel_y0; float rf_vel_x1, rf_vel_y1; #define ROBOT_START_Y (15.0*inches) #define HEADING_ALIGNMENT_TAU 0.1 void service_rf() { if (rf_x0 != last_rf_x0 || rf_y0 != last_rf_y0) { /* Robot 0 has moved. */ if (rf_update_count0 > 0) { rf_vel_x0 = (float)(rf_x0 - last_rf_x0); /* Assume 1 second update rate */ rf_vel_y0 = (float)(rf_y0 - last_rf_y0); /* Assume 1 second update rate */ } else { /*if ((rf_y0 < 0 && team_color==WHITE) || (rf_y0 > 0 && team_color==BLACK)) our_rf_number = 0;*/ first_rf_x0 = rf_x0; first_rf_y0 = rf_y0; } last_rf_x0 = rf_x0; last_rf_y0 = rf_y0; rf_update_count0++; decide_which_rf_team_is_us(); if (our_rf_number==0) { if (team_color==WHITE) position_y = (float)(rf_y0 - first_rf_y0) - ROBOT_START_Y; else /*team_color==BLACK)*/ position_y = (float)(rf_y0 - first_rf_y0) + ROBOT_START_Y; position_x = (float)(rf_x0 - first_rf_x0); heading = heading + HEADING_ALIGNMENT_TAU * (vector_to_heading(rf_vel_x0, rf_vel_y0)-heading); } } if (rf_x1 != last_rf_x1 || rf_y1 != last_rf_y1) { /* Robot 1 has moved. */ if (rf_update_count1 > 0) { rf_vel_x1 = (float)(rf_x1 - last_rf_x1); /* Assume 1 second update rate */ rf_vel_y1 = (float)(rf_y1 - last_rf_y1); /* Assume 1 second update rate */ } else { /*if ((rf_y1 > 0 && team_color==BLACK) || (rf_y1 < 0 && team_color==WHITE)) our_rf_number = 1;*/ first_rf_x1 = rf_x1; first_rf_y1 = rf_y1; } last_rf_x1 = rf_x1; last_rf_y1 = rf_y1; rf_update_count1++; decide_which_rf_team_is_us(); if (our_rf_number==1) { if (team_color==WHITE) position_y = (float)(rf_y1 - first_rf_y1) - ROBOT_START_Y; else /*team_color==BLACK)*/ position_y = (float)(rf_y1 - first_rf_y1) + ROBOT_START_Y; position_x = (float)(rf_x1 - first_rf_x1); heading = heading + HEADING_ALIGNMENT_TAU * (vector_to_heading(rf_vel_x0, rf_vel_y0)-heading); } } } void decide_which_rf_team_is_us() { /* Ideally, we can accurately determine which RF team is us if two position updates have been received: */ if (rf_update_count0 > 0 && rf_update_count1 > 0) { if (team_color==WHITE) { if (first_rf_y0 > first_rf_y1) our_rf_number = 1; else if (first_rf_y0 < first_rf_y1) our_rf_number = 0; } else /* team_color==BLACK */ { if (first_rf_y0 > first_rf_y1) our_rf_number = 0; else if (first_rf_y0 < first_rf_y1) our_rf_number = 1; } } else if (false) /* Otherwise, we have only a single position update to rely on: */ { if (rf_update_count0 > 0) /* Only update is for team 0 */ { if (team_color==WHITE) { if (first_rf_y0 > 0) our_rf_number = 1; else /*first_rf_y0 < 0*/ our_rf_number = 0; } else /*team_color==BLACK*/ { if (first_rf_y0 > 0) our_rf_number = 0; else /*first_rf_y0 < 0*/ our_rf_number = 1; } } else if (rf_update_count1 > 0) /* Only update is for team 1 */ { if (team_color==WHITE) { if (first_rf_y1 > 0) our_rf_number = 0; else /*first_rf_y1 < 0*/ our_rf_number = 1; } else /*team_color==BLACK*/ { if (first_rf_y1 > 0) our_rf_number = 1; else /*first_rf_y1 < 0*/ our_rf_number = 0; } } } } void init_globals() { uint8 n; rf_update_count0 = 0; rf_update_count1 = 0; last_rf_x0 = rf_x0; last_rf_y0 = rf_y0; last_rf_x1 = rf_x1; last_rf_y1 = rf_y1; last_timestamp = 0L; for (n=0; n<total_clusters; n++) virgin_clusters[n] = true; } long last_timestamp; float dt; void service_timer() { long timestamp; timestamp = mseconds(); dt = (float)(timestamp - last_timestamp) / 1000.0; last_timestamp = timestamp; } void ex_update_display() { char color[4]; clear_lcd(); /* ---------------- G22 R22 12Hz N O RF1 W 60 H-180 * ---------------- */ printf("R"); printint2(balls_red); printf("G"); printint2(balls_green); if (control_frequency<10) printf(" "); printf("%d", control_frequency); printf("Hz %d %c", navigate_state, initial_orientation[orientation_code]); if (team_color==WHITE) color = "W"; else color = "B"; printf("RF%d %s ", our_rf_number, color); printint2(60 - float_to_int(seconds())); printf("H%d", float_to_int(heading * RADIANS_TO_DEGREES)); } void update_display() { char color[4]; clear_lcd(); /* ---------------- -400 -400 12 N O 1 W 3r 4g H-180* ---------------- */ /* printf("R"); printint2(balls_red); printf("G"); printint2(balls_green); */ /*if (position_x >= 0.0) printf(" ");*/ printint3(float_to_int(position_x)); /*if (position_y >= 0.0) printf(" ");*/ printint3(float_to_int(position_y)); if (control_frequency<10) printf(" "); printf("%d", control_frequency); printf(" %d %c", navigate_state, initial_orientation[orientation_code]); if (team_color==WHITE) color = "W"; else color = "B"; printf("%d %s ", our_rf_number, color); /*printf("%d %s %dr %dg", our_rf_number, color, balls_red, balls_green);*/ printf("DH%d ", float_to_int(desired_heading * RADIANS_TO_DEGREES)); /* printint2(60 - float_to_int(seconds()));*/ printf("H%d", float_to_int(heading * RADIANS_TO_DEGREES)); } void make_first_plan() { if (team_color == WHITE) nearest_cluster = 5; else nearest_cluster = 0; /* dest_start_x = clusters_x[nearest_cluster]; dest_end_x = clusters_x[nearest_cluster]; */ dest_start_x = 0.0; dest_end_x = 0.0; if (team_color == WHITE) { /* dest_start_y = -triple_cluster_y; dest_end_y = white_scoring_area_y[1]; */ dest_start_y = -triple_cluster_y+cluster_side/2.0; dest_end_y = -3.0*feet; } else /*team_color==BLACK*/ { /* dest_start_y = triple_cluster_y; dest_end_y = -white_scoring_area_y[1]; */ dest_start_y = triple_cluster_y-cluster_side/2.0; dest_end_y = 3.0*feet; } motor(muncher_motor_port_a, -float_to_int(SAFE_MOTOR_MAXIMUM)+20); /* Push balls */ motor(muncher_motor_port_b, -float_to_int(SAFE_MOTOR_MAXIMUM)+20); } void make_new_plan() { if (mseconds() < 1000L) nearest_cluster = initial_target_cluster[orientation_code]; else nearest_cluster = find_nearest_cluster(virgin_clusters); find_nearest_entry_point(nearest_cluster, &dest_start_x, &dest_start_y); dest_end_x = twice(clusters_x[nearest_cluster])-dest_start_x; dest_end_y = twice(clusters_y[nearest_cluster])-dest_start_y; motor(muncher_motor_port_a, float_to_int(SAFE_MOTOR_MAXIMUM)+20); motor(muncher_motor_port_b, float_to_int(SAFE_MOTOR_MAXIMUM)+20); } void compete() { int control_count=0; long count_start=0L; /*make_new_plan();*/ make_first_plan(); enable_servos(); servo(SERVO_CLAW, CLAW_CENTER); /*servo(SERVO_CASTOR, CASTOR_TURN);*/ set_castor_angle(90.0*DEGREES_TO_RADIANS); servo(SERVO_BALL_GATE, CLOSE_REAR_GATE); while (!stop_button()) { service_timer(); service_sensors(); service_rf(); service_shaft_encoder(); navigate(); ball_sort(); if (mseconds() > 60000L) break; /* the match is over */ /* Measure control loop frequency */ control_count++; if (mseconds() - count_start >= 1000L) { control_frequency = control_count; control_count = 0; count_start = mseconds(); } update_display(); }; } void shutdown() { ao(); disable_servos(); /*printf("\nVICTORY!");*/ while(1) defer(); /*hog_processor();*/ } void find_nearest_scoring_region(float* score_x,float* score_y) { int n; float dist, min_dist; float y; min_dist = HUGE; for (n=0; n<scoring_areas_per_team; n++) { if (team_color == WHITE) y = white_scoring_area_y[n]; else y = -white_scoring_area_y[n]; dist = distance(position_x, position_y, white_scoring_area_x[n], y); if (dist < min_dist) { min_dist = dist; *score_x = white_scoring_area_x[n]; *score_y = white_scoring_area_y[n]; } } } void find_nearest_entry_point(int cluster_id, float* out_x, float* out_y) { float half_cluster_strip; float try_x[4]; float try_y[4]; uint8 n, winner; float dist, min_dist; min_dist = HUGE; half_cluster_strip = cluster_strip/2.0 + ROBOT_RADIUS; try_x[0] = clusters_x[cluster_id] + half_cluster_strip; try_y[0] = clusters_y[cluster_id]; try_x[1] = clusters_x[cluster_id] - half_cluster_strip; try_y[1] = clusters_y[cluster_id]; try_x[2] = clusters_x[cluster_id]; try_y[2] = clusters_y[cluster_id] - half_cluster_strip; try_x[3] = clusters_x[cluster_id]; try_y[3] = clusters_y[cluster_id] + half_cluster_strip; for (n=0; n<4; n++) { dist = distance(position_x, position_y, try_x[n], try_y[n]); if (dist < min_dist) { min_dist = dist; winner = n; } } *out_x = try_x[winner]; *out_y = try_y[winner]; } int find_nearest_cluster(bool selected[]) { int n; float dist; float min_dist = HUGE; int nearest_cluster = -1; for (n=0; n<total_clusters; n++) { if (selected[n]) { dist = distance(clusters_x[n], clusters_y[n], position_x, position_y); if (dist < min_dist) { min_dist = dist; nearest_cluster = n; } } } return nearest_cluster; } #define BALLS_GREEN_CAPACITY 7 #define BALLS_RED_CAPACITY 6 /* BEN's BALL-SORTING CODE: */ #define BS_NOTHING 0 #define BS_BALL 1 #define BS_BALL_DELAY 2 #define BS_CLAW 3 #define BS_CLAW_DELAY 4 #define BALL_NONE 0 #define BALL_RED 1 #define BALL_GREEN 2 #define CLAW_CENTER 2100 #define CLAW_LEFT 2930 #define CLAW_RIGHT 1270 #define CLAW_DELAY 400L /* this needs to be found by testing*/ #define BALL_DELAY 200L #define BALL_THRES_GREEN (170+10) /*#define BALL_THRES_RED 43*/ #define BALL_THRES_RED (85+10) int balls_red = 0; int balls_green = 0; long bs_delay_start_ball, bs_delay_start_claw; int sense_led; void ball_sort () { sense_led = safe_analog(SENSOR_BALL); if (state_bs == BS_NOTHING) bs_nothing(); else if (state_bs == BS_BALL) bs_ball(); else if (state_bs == BS_BALL_DELAY) bs_ball_delay(); else if (state_bs == BS_CLAW) bs_claw(); else if (state_bs == BS_CLAW_DELAY) bs_claw_delay(); else error("bad state_bs"); } void bs_nothing() { if (sense_led < BALL_THRES_GREEN) state_bs = BS_BALL; } void bs_ball() { bs_delay_start_ball = mseconds(); state_bs = BS_BALL_DELAY; } void bs_ball_delay() { if (mseconds() - bs_delay_start_ball >= BALL_DELAY) { state_bs = BS_CLAW; if (sense_led < BALL_THRES_RED) state_ball = BALL_RED; else state_ball = BALL_GREEN; } } void bs_claw() { if (state_ball == BALL_RED) { if (balls_red < BALLS_RED_CAPACITY) { servo(SERVO_CLAW, CLAW_RIGHT); balls_red ++; } else go_to_scoring_region_now(); } else if (state_ball == BALL_GREEN) { if (balls_green < BALLS_GREEN_CAPACITY) { servo(SERVO_CLAW, CLAW_LEFT); balls_green ++; } else go_to_scoring_region_now(); } else if (state_ball == BALL_NONE) servo(SERVO_CLAW, CLAW_CENTER); else error("bad state_ball"); bs_delay_start_claw = mseconds(); state_bs = BS_CLAW_DELAY; } void bs_claw_delay() { if (mseconds() - bs_delay_start_claw >= CLAW_DELAY) { if (state_ball == BALL_NONE) state_bs = BS_NOTHING; else if (state_ball == BALL_GREEN || state_ball == BALL_RED) { state_bs = BS_CLAW; state_ball = BALL_NONE; } } } void error(char message[]) { ao(); printf(message); while(1); }