Skip to content

Preflight check #177

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
vp13t merged 6 commits into from
Mar 9, 2026
Merged

Preflight check #177

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
1807d5a
Controller preflight check
vp13t Jan 17, 2026
2253b03
Mode enums
vp13t Jan 25, 2026
00ecb99
Enforce preflight when robot starts before controller
vp13t Feb 22, 2026
0963d31
Fix safety initialization requirement
vp13t Feb 22, 2026
f3e76b9
Log msg
vp13t Feb 22, 2026
6d11bac
Print mode string names
vp13t Feb 26, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
94 changes: 60 additions & 34 deletions src/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,9 +123,11 @@ int main() {
transmitter = new ET16S;
}

transmitter->init();


// initialize objects
can.init();
transmitter->init();
comms_layer.init();

ref = sensor_manager.get_ref();
Expand Down Expand Up @@ -185,9 +187,10 @@ int main() {
int count_one = 0;

// whether we are in hive mode or not
bool hive_toggle = false;
bool safety_toggle = false;
bool not_safety_mode = false;
enum Mode { SAFETY, TEENSY, HIVE} mode = SAFETY;
std::string mode_strs[3] = {"SAFETY", "TEENSY", "HIVE"};
bool mode_changed = false;
bool safety_initialized = false; // whether the transmitter has been in safety mode since boot, used to prevent the teensy from starting in a non-safety mode
bool last_gimbal_power = false; // used to detect gimbal power changes
bool last_loop_slow = false; // used to detect multiple slow loops in a row
int slow_loop_counter = 0; // used to count slow loops in a row
Expand Down Expand Up @@ -304,7 +307,7 @@ int main() {
if (dt2 > 0.1)
dt2 = 0;
// check if the shooter is active
if (not_safety_mode && ref->ref_data.robot_performance.shooter_power_active)
if (mode != SAFETY && ref->ref_data.robot_performance.shooter_power_active)
feed += feeder_target * dt2;
target_state[6][0] = (int)feed;
} else {
Expand All @@ -329,28 +332,23 @@ int main() {

// if the left switch is all the way down use Hive controls

if (transmitter->get_l_switch() == SwitchPos::BACKWARD) {
if (mode == HIVE) {
// hid_incoming.get_target_state(target_state);
memcpy(target_state, comms_layer.get_hive_data().target_state.state, sizeof(target_state));
last_feed = target_state[6][0];
// if you just switched to hive controls, set the reference to the
// current state'
if (hive_toggle) {
if (mode_changed) {
governor.set_reference(temp_state);
hive_toggle = false;
}
}

// when in teensy control mode reset hive toggle
if (transmitter->get_l_switch() == SwitchPos::MIDDLE) {
if (!hive_toggle || !safety_toggle) {
pos_offset_x = temp_state[0][0];
pos_offset_y = temp_state[1][0];
feed = last_feed;
governor.set_reference(temp_state);
}
hive_toggle = true;
safety_toggle = true;
if (mode == TEENSY && mode_changed) {
pos_offset_x = temp_state[0][0];
pos_offset_y = temp_state[1][0];
feed = last_feed;
governor.set_reference(temp_state);
}

// print transmitter
Expand All @@ -365,7 +363,7 @@ int main() {
// }

// override temp state if needed. Dont override in teensy mode so the sentry doesnt move during inspection
if (comms_layer.get_hive_data().override_state.active && !(transmitter->get_l_switch() == SwitchPos::MIDDLE)) {
if (comms_layer.get_hive_data().override_state.active && mode != TEENSY) {
// clear the request
comms_layer.get_hive_data().override_state.active = false;

Expand All @@ -380,9 +378,8 @@ int main() {
estimator_manager.step(temp_state, temp_micro_state, override_request);
override_request = false;

if ((feed - temp_state[6][0] > 2 && transmitter->get_l_switch() == SwitchPos::MIDDLE) ||
(comms_layer.get_hive_data().target_state.state[6][0] - temp_state[6][0] > 2 &&
transmitter->get_l_switch() == SwitchPos::BACKWARD)) {
if ((feed - temp_state[6][0] > 2 && mode == TEENSY) ||
(comms_layer.get_hive_data().target_state.state[6][0] - temp_state[6][0] > 2 && mode == HIVE)) {
Serial.printf("Feeder is lowkey jammed. current ball count: %f, feed: %f, hive target: %f\n",
temp_state[6][0], feed, comms_layer.get_hive_data().target_state.state[6][0]);
feed = temp_state[6][0] + 1;
Expand Down Expand Up @@ -469,18 +466,43 @@ int main() {
last_gimbal_power = ref->ref_data.robot_performance.gimbol_power_active;
bool gimbal_power_recently_turned_on = gimbal_power_timer.get_elapsed_micros_no_restart() < 3000000;

not_safety_mode =
(transmitter->is_connected() &&
(transmitter->get_l_switch() == SwitchPos::BACKWARD || transmitter->get_l_switch() == SwitchPos::MIDDLE) &&
config_layer.is_configured() && !is_slow_loop && ref->ref_data.robot_performance.gimbol_power_active &&
!gimbal_power_recently_turned_on);
// SAFETY MODE
if (not_safety_mode) {
// SAFETY OFF
can.write();
// Serial.printf("Can write\n");
// Serial.printf("Can write\n");
} else {
mode_changed = false;
enum Mode requested_mode = SAFETY;
if (transmitter->get_l_switch() == SwitchPos::FORWARD) {
requested_mode = SAFETY;
safety_initialized = true;
} else if (transmitter->get_l_switch() == SwitchPos::MIDDLE) {
requested_mode = TEENSY;
} else if (transmitter->get_l_switch() == SwitchPos::BACKWARD) {
requested_mode = HIVE;
}
if (requested_mode != mode && mode == SAFETY) {
if (!safety_initialized) {
Serial.println("Failed to disengage safety mode. Safety has not been initialized since boot.");
requested_mode = SAFETY;
}
if (!transmitter->is_connected() || !config_layer.is_configured() || is_slow_loop ||
!ref->ref_data.robot_performance.gimbol_power_active || gimbal_power_recently_turned_on) {
Serial.println("Failed to disengage safety mode. Robot not ready.");
requested_mode = SAFETY;
}
//make sure there is no spin
else if(transmitter->get_wheel() != 0) {
Serial.println("Failed to disengage safety mode. Wheel must be set to 0.");
requested_mode = SAFETY;
}
//make sure flywheels and the feeder are off
else if(transmitter->get_r_switch() != SwitchPos::BACKWARD) {
Serial.println("Failed to disengage safety mode. Right switch must be in backward position (shooter off).");
requested_mode = SAFETY;
}
}
if (requested_mode != mode) {
Serial.printf("Switching mode from %s to %s\n", mode_strs[mode].c_str(), mode_strs[requested_mode].c_str());
mode = requested_mode;
mode_changed = true;
}
if (mode == SAFETY) {
// SAFETY ON
// TODO: Reset all controller integrators here
can.issue_safety_mode();
Expand All @@ -491,7 +513,11 @@ int main() {
: (int)floor(temp_state[6][0]); // reset feed to the current state
last_feed = feed; // reset last feed to the current state
// Serial.printf("Can zero\n");
safety_toggle = false; // reset hive toggle
} else {
// SAFETY OFF
can.write();
// Serial.printf("Can write\n");
// Serial.printf("Can write\n");
}

// LED heartbeat -- linked to loop count to reveal slowdowns and
Expand Down