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;
}
}
