static inline void racetrack_state_car_test_run(void) { // in this mode, the test menu should: // a) change the lane states between LANE_STOPPED and LANE_CALIBRATE // b) set the desired lane speeds sol_enable(SOL_RACE_DIRECTION); }
void claw_bump_left (void) { if (claw_location != CLAW_LEFT){ disable_interrupts (); claw_timer = 1; claw_state = CLAW_FORWARD; claw_location = CLAW_UNKNOWN; sol_disable (SOL_CLAW_RIGHT); sol_enable (SOL_CLAW_LEFT); task_recreate_gid (GID_CLAW_B_MONITOR, claw_bump_monitor); enable_interrupts (); } }
static inline void racetrack_state_race_enter(void) { sol_enable(SOL_RACE_DIRECTION); racetrack_reset_track_state(); // lane is speed controlled by lane state, no need to set it here racetrack_lanes[LANE_LEFT].state = LANE_SEEK; racetrack_lanes[LANE_RIGHT].state = LANE_SEEK; }
static inline void racetrack_state_race_run(void) { // In this mode the game-code should increase the desired_encoder_count's value // the RTT will advance the cars so they reach the desired encoder count. sol_enable(SOL_RACE_DIRECTION); /* move this into lane state? */ if (racetrack_lanes[LANE_LEFT].desired_encoder_count > racetrack_lanes[LANE_LEFT].encoder_count) { racetrack_lanes[LANE_LEFT].state = LANE_SEEK; } if (racetrack_lanes[LANE_RIGHT].desired_encoder_count > racetrack_lanes[LANE_RIGHT].encoder_count) { racetrack_lanes[LANE_RIGHT].state = LANE_SEEK; } }
static inline void racetrack_state_car_test_enter(void) { sol_enable(SOL_RACE_DIRECTION); racetrack_reset_track_state(); set_lane_speed(LANE_LEFT, RACETRACK_SPEED_MEDIUM); set_lane_speed(LANE_RIGHT, RACETRACK_SPEED_MEDIUM); racetrack_lanes[LANE_LEFT].state = LANE_STOP; racetrack_lanes[LANE_RIGHT].state = LANE_STOP; }
/** The magnet switch handler is a frequently called function * that polls the magnet switches to see if a ball is on * top of the magnet, and quickly turns on the magnet when * it senses a ball there if it has been enabled to do so. */ static inline void magnet_rtt_switch_handler ( const U8 sw_magnet, const U8 sol_magnet, enum magnet_state *state, U8 *power_timer ) { /* rt_switch_poll is inverted because it is an opto */ if ((*state == MAG_ENABLED) && (!rt_switch_poll (sw_magnet))) { sol_enable (sol_magnet); *state = MAG_ON_POWER; *power_timer = MAG_POWER_TIME; } }
/** The magnet duty handler is a less-frequently called * function that turns on/off the magnet as necessary. * When a ball is being held, it uses duty cycling to avoid * burnout. */ static inline void magnet_rtt_duty_handler ( const U8 sw_magnet, const U8 sol_magnet, enum magnet_state *state, U8 *power_timer, U8 *hold_timer, bool *throw_enabled) { switch (*state) { case MAG_DISABLED: case MAG_ENABLED: sol_disable (sol_magnet); break; case MAG_ON_POWER: /* keep magnet on with high power */ /* switch to MAG_ON_HOLD fairly quickly though */ /* But leave solenoid enabled so it doesn't suffer * any drop */ --*power_timer; if (*power_timer == 0) { /* Inverted as it's an opto */ if (rt_switch_poll (sw_magnet)) { /* Grab failed */ sol_disable (sol_magnet); *throw_enabled = FALSE; *state = MAG_DISABLED; } else if (*throw_enabled) { *throw_enabled = FALSE; switch (sol_magnet) { case SOL_RIGHT_MAGNET: *hold_timer = DEFAULT_MAG_DROP_TIME_RIGHT \ + lower_right_magnet_swag; break; case SOL_LEFT_MAGNET: *hold_timer = DEFAULT_MAG_DROP_TIME_LEFT \ + left_magnet_swag; break; } /* switch to THROW_DROP */ sol_disable (sol_magnet); *state = MAG_THROW_DROP; } else { /* switch to HOLD */ *state = MAG_ON_HOLD; } } break; case MAG_THROW_POWER: /* stop power if the opto * stays closed for >20ms */ if (!rt_switch_poll (sw_magnet)) ++*hold_timer; if (--*power_timer == 0 || *hold_timer > 5) { sol_disable (sol_magnet); *state = MAG_DISABLED; } break; case MAG_ON_HOLD: --*hold_timer; /* keep magnet on with low power */ /* switch should remain closed in this state */ if (*hold_timer == 0) *state = MAG_DISABLED; else if ((*hold_timer % 2) != 0) sol_enable (sol_magnet); else sol_disable (sol_magnet); break; case MAG_THROW_DROP: /* Short delay to let the ball roll * down before applying a short pulse */ if (--*hold_timer == 0) { *power_timer = DEFAULT_MAG_THROW_TIME; *hold_timer = 0; /* switch to THROW_POWER */ sol_enable (sol_magnet); *state = MAG_THROW_POWER; } break; } }
void racetrack_state_calibrate_run(void) { // // handle transition // if (racetrack_calibrate_state != racetrack_calibrate_previous_state) { racetrack_calibrate_counter = 0; racetrack_calibrate_ticks_remaining = 1; // process the state this tick! racetrack_calibrate_previous_state = racetrack_calibrate_state; // process new state switch(racetrack_calibrate_state) { case RT_CALIBRATE_CAR_RETURN: /** * The first time this state is hit the cars can be anywhere on the track * * The goal is to return both cars to the start position */ sol_disable(SOL_RACE_DIRECTION); // backwards racetrack_reset_track_state(); set_lane_speed(LANE_LEFT, RACETRACK_SPEED_FASTEST); set_lane_speed(LANE_RIGHT, RACETRACK_SPEED_FASTEST); racetrack_lanes[LANE_LEFT].state = LANE_RETURN; racetrack_lanes[LANE_RIGHT].state = LANE_RETURN; break; case RT_CALIBRATE_LEFT_CAR_FORWARDS: /** * The first time this state is hit both cars will be at the start position. * * The goal is to drive the left car to the end of the track and stop */ sol_enable(SOL_RACE_DIRECTION); // forwards racetrack_reset_track_state(); racetrack_lanes[LANE_LEFT].state = LANE_CALIBRATE; break; case RT_CALIBRATE_RIGHT_CAR_FORWARDS: /** * The first time this state is hit the left car will be at the end of the track and the * right car will be at the start of the track. * * The goal is to drive the right car to the end of the track and stop */ sol_enable(SOL_RACE_DIRECTION); // forwards racetrack_lanes[LANE_RIGHT].state = LANE_CALIBRATE; break; case RT_CALIBRATE_LEFT_CAR_RETURN: /** * The first time this state is hit both cars will be at the end of the track * * The goal is to drive the left car to the end of the start of the track and stop */ sol_disable(SOL_RACE_DIRECTION); // backwards racetrack_reset_track_state(); racetrack_lanes[LANE_LEFT].state = LANE_RETURN; break; case RT_CALIBRATE_RIGHT_CAR_RETURN: /** * The first time this state is hit the left car will be at the start of the track and the * right car will be at the end of the track * * The goal is to drive the right car to the end of the start of the track and stop */ sol_disable(SOL_RACE_DIRECTION); // backwards racetrack_lanes[LANE_RIGHT].state = LANE_RETURN; break; default: // shut the compiler up break; } } racetrack_calibrate_ticks_remaining--; if (racetrack_calibrate_ticks_remaining != 0) { // TODO check if we need to update the SOL_RACE_DIRECTION more frequently, seems to work so far for me on my machine return; } // reset the timer racetrack_calibrate_ticks_remaining = RACETRACK_CALIBRATE_TICKS; // we've waited long enough now until we should check things again. racetrack_calibrate_counter++; switch(racetrack_calibrate_state) { case RT_CALIBRATE_CAR_RETURN: sol_disable(SOL_RACE_DIRECTION); // backwards if (switch_poll_logical (SW_LEFT_RACE_START) && switch_poll_logical (SW_RIGHT_RACE_START)) { racetrack_calibrate_state = RT_CALIBRATE_LEFT_CAR_FORWARDS; break; } // both cars should be moving backwards if (racetrack_calibrate_counter >= RACETRACK_CALIBRATE_TIMEOUT_COUNTER) { // both start of track optos should be on racetrack_calibration_failed(CC_CHECK_RACETRACK); break; } break; case RT_CALIBRATE_LEFT_CAR_FORWARDS: sol_enable(SOL_RACE_DIRECTION); // forwards if ((racetrack_encoder_mask & RT_EM_END_OF_TRACK_LEFT) != 0) { racetrack_calibrate_state = RT_CALIBRATE_RIGHT_CAR_FORWARDS; break; } if (racetrack_calibrate_counter >= RACETRACK_CALIBRATE_TIMEOUT_COUNTER) { racetrack_calibration_failed(CC_CHECK_LEFT_TRACK); break; } break; case RT_CALIBRATE_RIGHT_CAR_FORWARDS: sol_enable(SOL_RACE_DIRECTION); // forwards if ((racetrack_encoder_mask & RT_EM_END_OF_TRACK_RIGHT) != 0) { racetrack_calibrate_state = RT_CALIBRATE_LEFT_CAR_RETURN; break; } if (racetrack_calibrate_counter >= RACETRACK_CALIBRATE_TIMEOUT_COUNTER) { racetrack_calibration_failed(CC_CHECK_RIGHT_TRACK); break; } break; case RT_CALIBRATE_LEFT_CAR_RETURN: sol_disable(SOL_RACE_DIRECTION); // backwards if (switch_poll_logical (SW_LEFT_RACE_START)) { racetrack_calibrate_state = RT_CALIBRATE_RIGHT_CAR_RETURN; break; } // left car should be moving backwards if (racetrack_calibrate_counter >= RACETRACK_CALIBRATE_TIMEOUT_COUNTER) { // both start of track optos should be on racetrack_calibration_failed(CC_CHECK_LEFT_TRACK); break; } break; case RT_CALIBRATE_RIGHT_CAR_RETURN: sol_disable(SOL_RACE_DIRECTION); // backwards if (switch_poll_logical (SW_RIGHT_RACE_START)) { racetrack_calibration_complete(); break; } // right car should be moving backwards if (racetrack_calibrate_counter >= RACETRACK_CALIBRATE_TIMEOUT_COUNTER) { // both start of track optos should be on racetrack_calibration_failed(CC_CHECK_RIGHT_TRACK); break; } break; default: // shut the compiler up break; } }
/** * @param lane_number See LANE_* defines. */ void racetrack_process_lane(U8 lane_number) { // // handle transitions // switch(racetrack_lanes[lane_number].state) { case LANE_RETURN: // // detect at-start-of-track // if (switch_poll_logical(racetrack_lanes[lane_number].start_switch)) { racetrack_lanes[lane_number].state = LANE_STOP; } break; case LANE_SEEK: if (racetrack_lanes[lane_number].encoder_count >= racetrack_lanes[lane_number].desired_encoder_count) { racetrack_lanes[lane_number].state = LANE_STOP; } break; default: // shut the compiler up break; } switch(racetrack_lanes[lane_number].state) { case LANE_SEEK: case LANE_CALIBRATE: // // detect end-of-track (usable) // if (racetrack_lanes[lane_number].encoder_count > RACETRACK_LENGTH_USABLE) { racetrack_lanes[lane_number].state = LANE_STOP; if (lane_number == LANE_LEFT) { racetrack_encoder_mask |= RT_EM_END_OF_TRACK_LEFT; } else { racetrack_encoder_mask |= RT_EM_END_OF_TRACK_RIGHT; } break; } case LANE_RETURN: // // detect end-of-track (limit) // // If the cars are pushed all the way to the end of the track, and then reversed we // should wait for the start-of-track opto to be switched on, if it's not switched // on and we should stop if the encoder has registered almost all of the track. if (racetrack_lanes[lane_number].encoder_count > RACETRACK_LENGTH_LIMIT) { racetrack_lanes[lane_number].state = LANE_STOP; break; } // // detect stall // // don't look for a stall condition if we've only just started to move, wait a bit first if (racetrack_stall_ignore_ticks_remaining > 0) { racetrack_stall_ignore_ticks_remaining--; break; } racetrack_stall_ticks_remaining--; if (racetrack_stall_ticks_remaining > 0) { break; } racetrack_stall_ticks_remaining = LANE_STALL_DETECT_TICKS; if ( (lane_number == LANE_LEFT && ((racetrack_encoder_mask & RT_EM_SEEN_LEFT) == 0)) || (lane_number == LANE_RIGHT && ((racetrack_encoder_mask & RT_EM_SEEN_RIGHT) == 0)) ) { racetrack_lanes[lane_number].state = LANE_STOP; // stalled! if (lane_number == LANE_LEFT) { racetrack_encoder_mask |= RT_EM_STALLED_LEFT; } else { racetrack_encoder_mask |= RT_EM_STALLED_RIGHT; } } else { // clear the encoder seen flag if (lane_number == LANE_LEFT) { racetrack_encoder_mask &= ~RT_EM_SEEN_LEFT; } else { racetrack_encoder_mask &= ~RT_EM_SEEN_RIGHT; } } break; default: // shut the compiler up break; } // // handle state // switch (racetrack_lanes[lane_number].state) { case LANE_STOP: sol_disable(racetrack_lanes[lane_number].solenoid); break; case LANE_SEEK: // adjust the speed depending on the difference between desired position and actual position // if they're close, use a slow speed, if they're far apart use a fast speed. // using a fast speed to move a short distance means the car will over-run it's desired position encoder_difference = racetrack_lanes[lane_number].desired_encoder_count - racetrack_lanes[lane_number].encoder_count; new_speed = racetrack_lanes[lane_number].speed; if (encoder_difference > 50) { new_speed = RACETRACK_SPEED_FASTEST; } else if (encoder_difference > 25) { new_speed = RACETRACK_SPEED_MEDIUM; } else { new_speed = RACETRACK_SPEED_SLOWEST; } if (new_speed < racetrack_lanes[lane_number].speed) { // go faster racetrack_lanes[lane_number].speed_ticks_remaining = 1; // cause the solenoid to enable now (see below) racetrack_lanes[lane_number].speed = new_speed; } else if (new_speed > racetrack_lanes[lane_number].speed) { // go slower racetrack_lanes[lane_number].speed_ticks_remaining = 2; // cause the solenoid to disable now (see below) racetrack_lanes[lane_number].speed = new_speed; } // follow though ... case LANE_CALIBRATE: case LANE_RETURN: if (racetrack_lanes[lane_number].state == LANE_RETURN) { // yes, because we followed though // slow down when we get near the start, to prevent belt slip if (racetrack_lanes[lane_number].encoder_count > RACETRACK_LENGTH_SLOWDOWN_IN_REVERSE) { racetrack_lanes[lane_number].speed = RACETRACK_SPEED_SLOW; } } // turn the solenoid on once every 'speed' ticks racetrack_lanes[lane_number].speed_ticks_remaining--; if (racetrack_lanes[lane_number].speed_ticks_remaining == 0) { racetrack_lanes[lane_number].speed_ticks_remaining = racetrack_lanes[lane_number].speed; if (!( ((racetrack_encoder_mask & RT_EM_STALLED_LEFT) > 0 && lane_number == LANE_LEFT) || ((racetrack_encoder_mask & RT_EM_STALLED_RIGHT) > 0 && lane_number == LANE_RIGHT) )) { // never enable the solenoid if the car has stalled sol_enable(racetrack_lanes[lane_number].solenoid); } } else { sol_disable(racetrack_lanes[lane_number].solenoid); } break; default: // shut the compiler up break; } }