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

Improve state flow

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-08 21:31:44 -07:00
parent a1cdca0799
commit a92cf5cb9c
1 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
main/ugv_main.cc
View File

@ -162,6 +162,7 @@ struct State {
comms->yaw_angle = ahrs_.getYaw();
UGV_State ugv_state = comms->ugv_state;
comms->Unlock();
UGV_State next_state = ugv_state;
switch (ugv_state) {
default:
@ -180,13 +181,13 @@ struct State {
outputs.left_motor = 0.0;
outputs.right_motor = 0.0;
if (not_old && not_invalid) {
comms->ugv_state = UGV_State::STATE_DRIVING;
next_state = UGV_State::STATE_DRIVING;
}
break;
}
case UGV_State::STATE_DRIVING: {
if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) {
comms->ugv_state = UGV_State::STATE_AQUIRING;
next_state = UGV_State::STATE_AQUIRING;
break;
}
@ -195,7 +196,7 @@ struct State {
if (tgt_dist <= MIN_DIST) {
ESP_LOGI(TAG, "Finished driving to target");
comms->ugv_state = UGV_State::STATE_FINISHED;
next_state = UGV_State::STATE_FINISHED;
break;
}
@ -204,10 +205,11 @@ struct State {
float angle_delta = tgt_bearing - cur_bearing;
if (angle_delta < 180.f) angle_delta += 360.f;
if (angle_delta > 180.f) angle_delta -= 360.f;
float angle_pwr = angle_delta * ANGLE_P;
float angle_pwr = clamp_mag(angle_delta * ANGLE_P, MAX_ANGLE_POWER);
float power = DRIVE_POWER;
outputs.left_motor = DRIVE_POWER + angle_pwr;
outputs.right_motor = DRIVE_POWER - angle_pwr;
outputs.left_motor = power - angle_pwr;
outputs.right_motor = power + angle_pwr;
break;
}
case UGV_State::STATE_TEST:
@ -221,7 +223,7 @@ struct State {
if (angle_delta < 180.f) angle_delta += 360.f;
if (angle_delta > 180.f) angle_delta -= 360.f;
float angle_pwr = clamp_mag(angle_delta * ANGLE_P, MAX_ANGLE_POWER);
float power = 0.0;
float power = 0.0;
outputs.left_motor = power - angle_pwr;
outputs.right_motor = power + angle_pwr;
@ -229,6 +231,10 @@ struct State {
break;
}
io->WriteOutputs(outputs);
comms->Lock();
comms->ugv_state = next_state;
comms->Unlock();
}
};