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