From 7226d8112e2fb11b128a4200942ea719cbf665cf Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Thu, 9 May 2019 20:01:51 -0700 Subject: [PATCH] lots of work on driving and fixing stuff --- e32_client/messages_pb2.py | 98 ++++++++++++++-- e32_client/ugv.py | 17 ++- main/messages.proto | 12 ++ main/ugv_comms.cc | 19 +++- main/ugv_comms.hh | 15 +-- main/ugv_main.cc | 228 ++++++++++++++++++++++++++++--------- 6 files changed, 313 insertions(+), 76 deletions(-) diff --git a/e32_client/messages_pb2.py b/e32_client/messages_pb2.py index 2e98610..731a5cd 100644 --- a/e32_client/messages_pb2.py +++ b/e32_client/messages_pb2.py @@ -21,7 +21,7 @@ DESCRIPTOR = _descriptor.FileDescriptor( package='uas.ugv.messages', syntax='proto3', serialized_options=_b('H\003'), - serialized_pb=_b('\n\x0emessages.proto\x12\x10uas.ugv.messages\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"y\n\nUGV_Status\x12*\n\x05state\x18\x01 \x01(\x0e\x32\x1b.uas.ugv.messages.UGV_State\x12,\n\x08location\x18\x02 \x01(\x0b\x32\x1a.uas.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\"c\n\x0bUGV_Message\x12.\n\x06status\x18\x01 \x01(\x0b\x32\x1c.uas.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"N\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12\x31\n\x04type\x18\x02 \x01(\x0e\x32#.uas.ugv.messages.GroundCommandType\"U\n\rGroundMessage\x12\x32\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1f.uas.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*f\n\tUGV_State\x12\x0e\n\nSTATE_IDLE\x10\x00\x12\x12\n\x0eSTATE_AQUIRING\x10\x01\x12\x11\n\rSTATE_DRIVING\x10\x02\x12\x12\n\x0eSTATE_FINISHED\x10\x03\x12\x0e\n\nSTATE_TEST\x10\x04*K\n\x11GroundCommandType\x12\x0f\n\x0b\x43MD_DISABLE\x10\x00\x12\x17\n\x13\x43MD_DRIVE_TO_TARGET\x10\x01\x12\x0c\n\x08\x43MD_TEST\x10\x02\x42\x02H\x03\x62\x06proto3') + serialized_pb=_b('\n\x0emessages.proto\x12\x10uas.ugv.messages\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"y\n\nUGV_Status\x12*\n\x05state\x18\x01 \x01(\x0e\x32\x1b.uas.ugv.messages.UGV_State\x12,\n\x08location\x18\x02 \x01(\x0b\x32\x1a.uas.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\"c\n\x0bUGV_Message\x12.\n\x06status\x18\x01 \x01(\x0b\x32\x1c.uas.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"2\n\x10\x44riveHeadingData\x12\x0f\n\x07heading\x18\x01 \x01(\x02\x12\r\n\x05power\x18\x02 \x01(\x02\"\x93\x01\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12\x31\n\x04type\x18\x02 \x01(\x0e\x32#.uas.ugv.messages.GroundCommandType\x12;\n\rdrive_heading\x18\x03 \x01(\x0b\x32\".uas.ugv.messages.DriveHeadingDataH\x00\x42\x06\n\x04\x64\x61ta\"U\n\rGroundMessage\x12\x32\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1f.uas.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*\xa6\x01\n\tUGV_State\x12\x0e\n\nSTATE_IDLE\x10\x00\x12\x12\n\x0eSTATE_AQUIRING\x10\x01\x12\x11\n\rSTATE_DRIVING\x10\x02\x12\x12\n\x0eSTATE_FINISHED\x10\x03\x12\x0e\n\nSTATE_TEST\x10\x04\x12\x12\n\x0eSTATE_FLIPPING\x10\x05\x12\x11\n\rSTATE_TURNING\x10\x06\x12\x17\n\x13STATE_DRIVE_HEADING\x10\x07*b\n\x11GroundCommandType\x12\x0f\n\x0b\x43MD_DISABLE\x10\x00\x12\x17\n\x13\x43MD_DRIVE_TO_TARGET\x10\x01\x12\x0c\n\x08\x43MD_TEST\x10\x02\x12\x15\n\x11\x43MD_DRIVE_HEADING\x10\x03\x42\x02H\x03\x62\x06proto3') ) _UGV_STATE = _descriptor.EnumDescriptor( @@ -50,11 +50,23 @@ _UGV_STATE = _descriptor.EnumDescriptor( name='STATE_TEST', index=4, number=4, serialized_options=None, type=None), + _descriptor.EnumValueDescriptor( + name='STATE_FLIPPING', index=5, number=5, + serialized_options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='STATE_TURNING', index=6, number=6, + serialized_options=None, + type=None), + _descriptor.EnumValueDescriptor( + name='STATE_DRIVE_HEADING', index=7, number=7, + serialized_options=None, + type=None), ], containing_type=None, serialized_options=None, - serialized_start=515, - serialized_end=617, + serialized_start=638, + serialized_end=804, ) _sym_db.RegisterEnumDescriptor(_UGV_STATE) @@ -77,11 +89,15 @@ _GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor( name='CMD_TEST', index=2, number=2, serialized_options=None, type=None), + _descriptor.EnumValueDescriptor( + name='CMD_DRIVE_HEADING', index=3, number=3, + serialized_options=None, + type=None), ], containing_type=None, serialized_options=None, - serialized_start=619, - serialized_end=694, + serialized_start=806, + serialized_end=904, ) _sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE) @@ -91,9 +107,13 @@ STATE_AQUIRING = 1 STATE_DRIVING = 2 STATE_FINISHED = 3 STATE_TEST = 4 +STATE_FLIPPING = 5 +STATE_TURNING = 6 +STATE_DRIVE_HEADING = 7 CMD_DISABLE = 0 CMD_DRIVE_TO_TARGET = 1 CMD_TEST = 2 +CMD_DRIVE_HEADING = 3 @@ -235,6 +255,44 @@ _UGV_MESSAGE = _descriptor.Descriptor( ) +_DRIVEHEADINGDATA = _descriptor.Descriptor( + name='DriveHeadingData', + full_name='uas.ugv.messages.DriveHeadingData', + filename=None, + file=DESCRIPTOR, + containing_type=None, + fields=[ + _descriptor.FieldDescriptor( + name='heading', full_name='uas.ugv.messages.DriveHeadingData.heading', index=0, + number=1, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='power', full_name='uas.ugv.messages.DriveHeadingData.power', index=1, + number=2, type=2, cpp_type=6, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=348, + serialized_end=398, +) + + _GROUNDCOMMAND = _descriptor.Descriptor( name='GroundCommand', full_name='uas.ugv.messages.GroundCommand', @@ -256,6 +314,13 @@ _GROUNDCOMMAND = _descriptor.Descriptor( message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, serialized_options=None, file=DESCRIPTOR), + _descriptor.FieldDescriptor( + name='drive_heading', full_name='uas.ugv.messages.GroundCommand.drive_heading', index=2, + number=3, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR), ], extensions=[ ], @@ -267,9 +332,12 @@ _GROUNDCOMMAND = _descriptor.Descriptor( syntax='proto3', extension_ranges=[], oneofs=[ + _descriptor.OneofDescriptor( + name='data', full_name='uas.ugv.messages.GroundCommand.data', + index=0, containing_type=None, fields=[]), ], - serialized_start=348, - serialized_end=426, + serialized_start=401, + serialized_end=548, ) @@ -302,8 +370,8 @@ _GROUNDMESSAGE = _descriptor.Descriptor( name='ground_message', full_name='uas.ugv.messages.GroundMessage.ground_message', index=0, containing_type=None, fields=[]), ], - serialized_start=428, - serialized_end=513, + serialized_start=550, + serialized_end=635, ) _UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE @@ -316,6 +384,10 @@ _UGV_MESSAGE.oneofs_by_name['ugv_message'].fields.append( _UGV_MESSAGE.fields_by_name['command_ack']) _UGV_MESSAGE.fields_by_name['command_ack'].containing_oneof = _UGV_MESSAGE.oneofs_by_name['ugv_message'] _GROUNDCOMMAND.fields_by_name['type'].enum_type = _GROUNDCOMMANDTYPE +_GROUNDCOMMAND.fields_by_name['drive_heading'].message_type = _DRIVEHEADINGDATA +_GROUNDCOMMAND.oneofs_by_name['data'].fields.append( + _GROUNDCOMMAND.fields_by_name['drive_heading']) +_GROUNDCOMMAND.fields_by_name['drive_heading'].containing_oneof = _GROUNDCOMMAND.oneofs_by_name['data'] _GROUNDMESSAGE.fields_by_name['command'].message_type = _GROUNDCOMMAND _GROUNDMESSAGE.oneofs_by_name['ground_message'].fields.append( _GROUNDMESSAGE.fields_by_name['command']) @@ -323,6 +395,7 @@ _GROUNDMESSAGE.fields_by_name['command'].containing_oneof = _GROUNDMESSAGE.oneof DESCRIPTOR.message_types_by_name['Location'] = _LOCATION DESCRIPTOR.message_types_by_name['UGV_Status'] = _UGV_STATUS DESCRIPTOR.message_types_by_name['UGV_Message'] = _UGV_MESSAGE +DESCRIPTOR.message_types_by_name['DriveHeadingData'] = _DRIVEHEADINGDATA DESCRIPTOR.message_types_by_name['GroundCommand'] = _GROUNDCOMMAND DESCRIPTOR.message_types_by_name['GroundMessage'] = _GROUNDMESSAGE DESCRIPTOR.enum_types_by_name['UGV_State'] = _UGV_STATE @@ -350,6 +423,13 @@ UGV_Message = _reflection.GeneratedProtocolMessageType('UGV_Message', (_message. )) _sym_db.RegisterMessage(UGV_Message) +DriveHeadingData = _reflection.GeneratedProtocolMessageType('DriveHeadingData', (_message.Message,), dict( + DESCRIPTOR = _DRIVEHEADINGDATA, + __module__ = 'messages_pb2' + # @@protoc_insertion_point(class_scope:uas.ugv.messages.DriveHeadingData) + )) +_sym_db.RegisterMessage(DriveHeadingData) + GroundCommand = _reflection.GeneratedProtocolMessageType('GroundCommand', (_message.Message,), dict( DESCRIPTOR = _GROUNDCOMMAND, __module__ = 'messages_pb2' diff --git a/e32_client/ugv.py b/e32_client/ugv.py index 39f153e..77de712 100755 --- a/e32_client/ugv.py +++ b/e32_client/ugv.py @@ -35,13 +35,16 @@ class UGVComms(E32): data = msg.SerializeToString() self.write_base64(data) - def write_command(self, cmd_type: messages.GroundCommandType, retry=True): + def write_command(self, command, retry=True): cmdid = self.next_command_id self.next_command_id += 1 gmsg = messages.GroundMessage() + if type(command) is int: + gmsg.command.type = command + else: + gmsg.command.CopyFrom(command) gmsg.command.id = cmdid - gmsg.command.type = cmd_type self.write_message(gmsg) last_write_time = time.time() if not retry: @@ -117,9 +120,13 @@ def main(): time.sleep(0.2) try: while True: - if ugv.last_status is None or ugv.last_status.state is not messages.STATE_TEST: - ugv.write_command(messages.CMD_TEST) - time.sleep(2.) + if ugv.last_status is None or ugv.last_status.state is not messages.STATE_DRIVE_HEADING: + cmd = messages.GroundCommand() + cmd.type = messages.CMD_DRIVE_HEADING + cmd.drive_heading.heading = -90.0 + cmd.drive_heading.power = 0.2 + ugv.write_command(cmd) + time.sleep(1.0) except KeyboardInterrupt: ugv.write_command(messages.CMD_DISABLE) print("exiting...") diff --git a/main/messages.proto b/main/messages.proto index 2dd2878..92dc51a 100644 --- a/main/messages.proto +++ b/main/messages.proto @@ -10,6 +10,9 @@ enum UGV_State { STATE_DRIVING = 2; STATE_FINISHED = 3; STATE_TEST = 4; + STATE_FLIPPING = 5; + STATE_TURNING = 6; + STATE_DRIVE_HEADING = 7; } message Location { @@ -36,11 +39,20 @@ enum GroundCommandType { CMD_DISABLE = 0; CMD_DRIVE_TO_TARGET = 1; CMD_TEST = 2; + CMD_DRIVE_HEADING = 3; +} + +message DriveHeadingData { + float heading = 1; + float power = 2; } message GroundCommand { uint32 id = 1; GroundCommandType type = 2; + oneof data { + DriveHeadingData drive_heading = 3; + } } message GroundMessage { diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc index 05099ae..49b82d6 100644 --- a/main/ugv_comms.cc +++ b/main/ugv_comms.cc @@ -144,9 +144,9 @@ void CommsClass::RunTask() { } #else // receiving packet data now - rx_len = lora.ReadLn(rx_buf, PACKET_RX_TIMEOUT); + rx_len = lora.ReadLn(rx_buf, delay_ticks); if (rx_len <= 0) { - ESP_LOGE(TAG, "timeout for packet rx"); + ESP_LOGI(TAG, "timeout for packet rx"); lora.Flush(); } else { ESP_LOGV(TAG, "rx data: %.*s", rx_buf.size(), rx_buf.c_str()); @@ -200,6 +200,7 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) { xSemaphoreTake(mutex, pdMS_TO_TICKS(10)); last_packet_tick = xTaskGetTickCount(); xSemaphoreGive(mutex); + ESP_LOGI(TAG, "rx base64 message: %.*s", data_size, data); int ret; size_t decoded_size = (data_size * 4 + 2) / 3; @@ -209,7 +210,7 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) { ret = mbedtls_base64_decode(decoded, decoded_size, &decoded_len, data, data_size); if (ret != 0) { - ESP_LOGE(TAG, "error with base64 decode: %d", ret); + ESP_LOGE(TAG, "base64 decode error: %d", ret); delete[] decoded; return; } @@ -244,6 +245,16 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) { ugv_state = messages::STATE_AQUIRING; break; case messages::CMD_TEST: ugv_state = messages::STATE_TEST; break; + case messages::CMD_DRIVE_HEADING: { + if (command.has_drive_heading()) { + this->ugv_state = messages::STATE_DRIVE_HEADING; + this->drive_heading = command.drive_heading(); + } else { + this->ugv_state = messages::STATE_IDLE; + ESP_LOGE(TAG, "invalid CMD_DRIVE_HEADING"); + } + break; + } default: ESP_LOGW(TAG, "unhandled command type: %d", command.type()); xSemaphoreGive(mutex); @@ -265,7 +276,7 @@ esp_err_t CommsClass::SendPacket(const uint8_t *data, size_t size) { ESP_LOGE(TAG, "SendPacket size too long: %d", size); return ESP_ERR_INVALID_SIZE; } - size_t encoded_size = ((size + 2) / 3) * 4; + size_t encoded_size = ((size + 6) / 3) * 4; uint8_t *encoded = new uint8_t[encoded_size]; size_t encoded_len; int ret = diff --git a/main/ugv_comms.hh b/main/ugv_comms.hh index 873cdce..d98cf8d 100644 --- a/main/ugv_comms.hh +++ b/main/ugv_comms.hh @@ -33,13 +33,14 @@ class CommsClass { uint8_t ReadLnaGain(); public: - SemaphoreHandle_t mutex; - messages::Location location; - float yaw_angle; - messages::UGV_State ugv_state; - TickType_t last_packet_tick; - int32_t last_packet_rssi; - int8_t last_packet_snr; + SemaphoreHandle_t mutex; + messages::Location location; + float yaw_angle; + messages::UGV_State ugv_state; + TickType_t last_packet_tick; + int32_t last_packet_rssi; + int8_t last_packet_snr; + messages::DriveHeadingData drive_heading; private: #ifdef COMMS_SX127X diff --git a/main/ugv_main.cc b/main/ugv_main.cc index e77c89b..ed8f9e2 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -21,35 +21,29 @@ SemaphoreHandle_t i2c_mutex; } constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; +constexpr float LOOP_PERIOD_S = 1000000.f / static_cast(LOOP_PERIOD_US); static const float PI = 3.1415926535897932384626433832795028841971693993751058209749445923078164062; -extern "C" void OnTimeout(void *arg); - -void UpdateLocationFromGPS(comms::messages::Location &location, - const io::GpsData & gps_data) { - location.set_fix_quality(gps_data.fix_quality); - location.set_latitude(gps_data.latitude); - location.set_longitude(gps_data.longitude); - location.set_altitude(gps_data.altitude); -} - static const float RAD_PER_DEG = PI / 180.f; // Radius of earth in meters static const float EARTH_RAD = 6372795.f; static const float DRIVE_POWER = 0.5; static const float ANGLE_P = 0.05; -static const float MAX_ANGLE_POWER = 0.3; +static const float ANGLE_I = 0.00; +static const float ANGLE_D = -0.05; +static const float MAX_ANGLE_POWER = 0.5; 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; +extern "C" void OnTimeout(void *arg); + +void UpdateLocationFromGPS(comms::messages::Location &location, + const io::GpsData & gps_data) { + location.set_fix_quality(gps_data.fix_quality); + location.set_latitude(gps_data.latitude); + location.set_longitude(gps_data.longitude); + location.set_altitude(gps_data.altitude); } struct LatLong { @@ -96,19 +90,134 @@ struct LatLong { } }; +class PIDController { + public: + explicit PIDController(float dt, float kp, float ki = 0., float kd = 0.); + + void MaxOutput(float max_output) { max_output_ = max_output; } + float MaxOutput() const { return max_output_; } + + void Setpoint(float setpoint) { setpoint_ = setpoint; } + float Setpoint() const { return setpoint_; } + + void Input(float input) { input_ = input; } + float Input() const { return input_; }; + + float Error() const { + float error = setpoint_ - input_; + // TODO: have this be configurable + while (error < 180.f) error += 360.f; + while (error > 180.f) error -= 360.f; + return error; + } + float Output() const { return output_; }; + + float Update(); + float Update(float input); + float Update(float input, float setpoint); + + void Reset(); + void Enable(bool enable = true) { enabled_ = enable; } + void Disable() { Enable(false); } + bool Enabled() const { return enabled_; } + + private: + static float clamp_mag(float x, float mag); + + float dt_; + float kp_; + float ki_; + float kd_; + float max_output_; + + bool enabled_; + float setpoint_; + float input_; + float output_; + + float integral_; + float last_error_; +}; + +float PIDController::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; +} + +PIDController::PIDController(float dt, float kp, float ki, float kd) + : dt_(dt), + kp_(kp), + ki_(ki), + kd_(kd), + max_output_(INFINITY), + enabled_(false), + setpoint_(0), + input_(0), + output_(0), + integral_(0), + last_error_(0) {} + +void PIDController::Reset() { + enabled_ = false; + setpoint_ = 0.; + input_ = 0.; + output_ = 0.; + integral_ = 0.; + last_error_ = NAN; +} + +float PIDController::Update() { + output_ = 0.; + if (!enabled_) { + return output_; + } + float error = Error(); + + integral_ += error * dt_; + + output_ += kp_ * error; + output_ += ki_ * integral_; + if (!isnan(last_error_)) { + output_ += kd_ * (error - last_error_); + } + output_ = clamp_mag(output_, max_output_); + + last_error_ = error; + return output_; +} + +float PIDController::Update(float input) { + Input(input); + return Update(); +} + +float PIDController::Update(float input, float setpoint) { + Setpoint(setpoint); + return Update(input); +} + struct State { public: CommsClass * comms; IOClass * io; DisplayClass * display; esp_timer_handle_t timer_handle; - io::Inputs inputs; - io::Outputs outputs; - int64_t last_print; - Madgwick ahrs_; - LatLong target; - State() : target{34.069022, -118.443067} { + io::Inputs inputs; + io::Outputs outputs; + int64_t last_print; + Madgwick ahrs_; + LatLong target; + PIDController angle_controller_; + + State() + : target{34.069022, -118.443067}, + angle_controller_(LOOP_PERIOD_S, ANGLE_P) { + angle_controller_.MaxOutput(MAX_ANGLE_POWER); comms = new CommsClass(); io = new IOClass(); display = new DisplayClass(comms); @@ -118,8 +227,7 @@ struct State { esp_timer_init(); i2c_mutex = xSemaphoreCreateMutex(); - ahrs_.begin(1000000.f / - static_cast(LOOP_PERIOD_US)); // rough sample frequency + ahrs_.begin(LOOP_PERIOD_S); // rough sample frequency io->Init(); comms->Init(); @@ -164,23 +272,44 @@ struct State { comms->Unlock(); UGV_State next_state = ugv_state; + angle_controller_.Input(ahrs_.getYaw()); + float drive_power = 0.; + outputs.left_motor = 0.0; + outputs.right_motor = 0.0; + switch (ugv_state) { default: ESP_LOGW(TAG, "unhandled state: %d", ugv_state); // fall through case UGV_State::STATE_IDLE: - case UGV_State::STATE_FINISHED: - outputs.left_motor = 0.0; - outputs.right_motor = 0.0; - break; + case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break; case UGV_State::STATE_AQUIRING: { + angle_controller_.Disable(); TickType_t current_tick = xTaskGetTickCount(); TickType_t ticks_since_gps = current_tick - inputs.gps.last_update; bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000); bool not_invalid = inputs.gps.fix_quality != io::GPS_FIX_INVALID; - outputs.left_motor = 0.0; - outputs.right_motor = 0.0; if (not_old && not_invalid) { + next_state = UGV_State::STATE_TURNING; + } + break; + } + case UGV_State::STATE_FLIPPING: { + angle_controller_.Disable(); + break; + } + case UGV_State::STATE_TURNING: { + if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) { + next_state = UGV_State::STATE_AQUIRING; + break; + } + + LatLong current_pos = {inputs.gps.latitude, inputs.gps.longitude}; + float tgt_bearing = current_pos.bearing_toward(target); + angle_controller_.Enable(); + angle_controller_.Setpoint(tgt_bearing); + + if (fabs(angle_controller_.Error()) <= 5.0) { next_state = UGV_State::STATE_DRIVING; } break; @@ -201,15 +330,8 @@ struct State { } float tgt_bearing = current_pos.bearing_toward(target); - 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 = DRIVE_POWER; - - outputs.left_motor = power - angle_pwr; - outputs.right_motor = power + angle_pwr; + angle_controller_.Enable(); + angle_controller_.Setpoint(tgt_bearing); break; } case UGV_State::STATE_TEST: @@ -217,19 +339,23 @@ struct State { outputs.left_motor = sinf(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; + angle_controller_.Enable(); + angle_controller_.Setpoint(90.0); #endif break; + case UGV_State::STATE_DRIVE_HEADING: + angle_controller_.Enable(); + angle_controller_.Setpoint(comms->drive_heading.heading()); + drive_power = comms->drive_heading.power(); + break; + } + + if (angle_controller_.Enabled()) { + float angle_pwr = angle_controller_.Update(); + outputs.left_motor = drive_power + angle_pwr; + outputs.right_motor = drive_power - angle_pwr; } + io->WriteOutputs(outputs); comms->Lock();