amikhalev/uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add flip/unflip logic

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-15 17:13:16 -07:00
parent 25cc0d0bc3
commit 50fd45ce27
1 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
main/ugv_main.cc
View File

@ -290,6 +290,10 @@ struct State {
outputs.left_motor = 0.0; outputs.left_motor = 0.0;
outputs.right_motor = 0.0; outputs.right_motor = 0.0;
float pitch = ahrs_.getPitch();
bool is_upside_down = (pitch > 90.) || (pitch < -90.);
switch (ugv_state) { switch (ugv_state) {
default: default:
ESP_LOGW(TAG, "unhandled state: %d", ugv_state); ESP_LOGW(TAG, "unhandled state: %d", ugv_state);
@ -297,6 +301,10 @@ struct State {
case UGV_State::STATE_IDLE: case UGV_State::STATE_IDLE:
case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break; case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break;
case UGV_State::STATE_AQUIRING: { case UGV_State::STATE_AQUIRING: {
if (is_upside_down) {
next_state = UGV_State::STATE_FLIPPING;
break;
}
angle_controller_.Disable(); angle_controller_.Disable();
TickType_t current_tick = xTaskGetTickCount(); TickType_t current_tick = xTaskGetTickCount();
TickType_t ticks_since_gps = current_tick - inputs.gps.last_update; TickType_t ticks_since_gps = current_tick - inputs.gps.last_update;
@ -309,9 +317,19 @@ struct State {
} }
case UGV_State::STATE_FLIPPING: { case UGV_State::STATE_FLIPPING: {
angle_controller_.Disable(); angle_controller_.Disable();
outputs.left_motor = -1.0;
outputs.right_motor = -1.0;
if (!is_upside_down) {
next_state = UGV_State::STATE_AQUIRING;
break;
}
break; break;
} }
case UGV_State::STATE_TURNING: { case UGV_State::STATE_TURNING: {
if (is_upside_down) {
next_state = UGV_State::STATE_FLIPPING;
break;
}
if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) { if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) {
next_state = UGV_State::STATE_AQUIRING; next_state = UGV_State::STATE_AQUIRING;
break; break;
@ -328,6 +346,10 @@ struct State {
break; break;
} }
case UGV_State::STATE_DRIVING: { case UGV_State::STATE_DRIVING: {
if (is_upside_down) {
next_state = UGV_State::STATE_FLIPPING;
break;
}
if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) { if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) {
next_state = UGV_State::STATE_AQUIRING; next_state = UGV_State::STATE_AQUIRING;
break; break;