// This RTT needs to be FAST as it's scheduled frequently
void corvette_racetrack_encoder_rtt (void) {
racetrack_encoder_previous_mask = racetrack_encoder_mask;
// check the optos and update the bitmask
// left lane
if (rt_switch_poll (SW_LEFT_RACE_ENCODER)) {
racetrack_encoder_mask |= (RT_EM_PREVIOUS_STATE_LEFT);
} else {
racetrack_encoder_mask &= ~RT_EM_PREVIOUS_STATE_LEFT;
}
if ((racetrack_encoder_mask & RT_EM_PREVIOUS_STATE_LEFT) != (racetrack_encoder_previous_mask & RT_EM_PREVIOUS_STATE_LEFT)) {
// encoder state changed
racetrack_lanes[LANE_LEFT].encoder_count++;
racetrack_encoder_mask |= (RT_EM_SEEN_LEFT);
}
// right lane
if (rt_switch_poll (SW_RIGHT_RACE_ENCODER)) {
racetrack_encoder_mask |= (RT_EM_PREVIOUS_STATE_RIGHT);
} else {
racetrack_encoder_mask &= ~RT_EM_PREVIOUS_STATE_RIGHT;
}
if ((racetrack_encoder_mask & RT_EM_PREVIOUS_STATE_RIGHT) != (racetrack_encoder_previous_mask & RT_EM_PREVIOUS_STATE_RIGHT)) {
// encoder state changed
racetrack_lanes[LANE_RIGHT].encoder_count++;
racetrack_encoder_mask |= (RT_EM_SEEN_RIGHT);
}
}
/** 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;
}
}
