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

add better test for driving

Browse Source
This commit is contained in:
Alex Mikhalev 2019-04-25 20:51:40 -07:00
parent 28bf68f4e6
commit a1cdca0799
2 changed files with 33 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
main/ugv_io.cc
View File

@ -78,7 +78,7 @@ void IOClass::WriteOutputs(const Outputs &outputs) {
ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1); ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1);
ERROR_CHECK(ret); ERROR_CHECK(ret);
bool left_dir, right_dir; bool left_dir, right_dir;
left_dir = outputs.left_motor < 0.f; left_dir = outputs.left_motor > 0.f;
right_dir = outputs.right_motor < 0.f; right_dir = outputs.right_motor < 0.f;
ret = gpio_set_level(MOTOR_LEFT_DIR, left_dir); ret = gpio_set_level(MOTOR_LEFT_DIR, left_dir);

35
main/ugv_main.cc
View File

@ -38,9 +38,19 @@ static const float RAD_PER_DEG = PI / 180.f;
// Radius of earth in meters // Radius of earth in meters
static const float EARTH_RAD = 6372795.f; static const float EARTH_RAD = 6372795.f;
static const float DRIVE_POWER = 0.5; static const float DRIVE_POWER = 0.5;
static const float ANGLE_P = 0.02; static const float ANGLE_P = 0.05;
static const float MIN_DIST = 10.0; static const float MAX_ANGLE_POWER = 0.3;
static const float MIN_DIST = 10.0;
float clamp_mag(float x, float max_mag) {
if (x > max_mag)
return max_mag;
else if (x < -max_mag)
return -max_mag;
else
return x;
}
struct LatLong { struct LatLong {
public: public:
@ -149,6 +159,7 @@ struct State {
comms->Lock(); comms->Lock();
UpdateLocationFromGPS(comms->location, inputs.gps); UpdateLocationFromGPS(comms->location, inputs.gps);
comms->yaw_angle = ahrs_.getYaw();
UGV_State ugv_state = comms->ugv_state; UGV_State ugv_state = comms->ugv_state;
comms->Unlock(); comms->Unlock();
@ -174,6 +185,11 @@ struct State {
break; break;
} }
case UGV_State::STATE_DRIVING: { case UGV_State::STATE_DRIVING: {
if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) {
comms->ugv_state = UGV_State::STATE_AQUIRING;
break;
}
LatLong current_pos = {inputs.gps.latitude, inputs.gps.longitude}; LatLong current_pos = {inputs.gps.latitude, inputs.gps.longitude};
float tgt_dist = current_pos.distance_to(target); float tgt_dist = current_pos.distance_to(target);
@ -195,8 +211,21 @@ struct State {
break; break;
} }
case UGV_State::STATE_TEST: case UGV_State::STATE_TEST:
#ifdef BASIC_TEST
outputs.left_motor = sinf(time_s * PI); outputs.left_motor = sinf(time_s * PI);
outputs.right_motor = cosf(time_s * PI); outputs.right_motor = cosf(time_s * PI);
#else
float tgt_bearing = 90.0;
float cur_bearing = ahrs_.getYaw();
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 = clamp_mag(angle_delta * ANGLE_P, MAX_ANGLE_POWER);
float power = 0.0;
outputs.left_motor = power - angle_pwr;
outputs.right_motor = power + angle_pwr;
#endif
break; break;
} }
io->WriteOutputs(outputs); io->WriteOutputs(outputs);