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

send config over rf link

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-15 21:21:17 -07:00
parent 9fe62e38af
commit 31f41c4b3c
6 changed files with 202 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
main/CMakeLists.txt
View File

@ -18,6 +18,8 @@ set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT})
set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls") set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls")
proto_generate_cpp(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS) proto_generate_cpp(messages.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
proto_generate_cpp(config.proto ${PB_OUT} PROTO_HDRS PROTO_SRCS)
message(STATUS "PROTO_SRCS: ${PROTO_SRCS}")
list(APPEND COMPONENT_SRCS ${PROTO_SRCS} ${PROTO_HDRS}) list(APPEND COMPONENT_SRCS ${PROTO_SRCS} ${PROTO_HDRS})
register_component() register_component()

20
main/config.proto Normal file
View File

@ -0,0 +1,20 @@
syntax = "proto3";
package ugv.config;
option optimize_for = LITE_RUNTIME;
message PidParams {
float kp = 1;
float ki = 2;
float kd = 3;
float max_output = 4;
float max_i_error = 5;
}
message Config {
float min_target_dist = 1;
float drive_power = 2;
PidParams angle_pid = 3;
float min_flip_pitch = 4;
}

13
main/messages.proto
View File

@ -1,6 +1,8 @@
syntax = "proto3"; syntax = "proto3";
package uas.ugv.messages; package ugv.messages;
import public "config.proto";
option optimize_for = LITE_RUNTIME; option optimize_for = LITE_RUNTIME;
@ -15,6 +17,11 @@ enum UGV_State {
STATE_DRIVE_HEADING = 7; STATE_DRIVE_HEADING = 7;
} }
message TargetLocation {
float latitude = 1;
float longitude = 2;
}
message Location { message Location {
uint32 fix_quality = 1; uint32 fix_quality = 1;
float latitude = 2; float latitude = 2;
@ -40,6 +47,8 @@ enum GroundCommandType {
CMD_DRIVE_TO_TARGET = 1; CMD_DRIVE_TO_TARGET = 1;
CMD_TEST = 2; CMD_TEST = 2;
CMD_DRIVE_HEADING = 3; CMD_DRIVE_HEADING = 3;
CMD_SET_TARGET = 4;
CMD_SET_CONFIG = 5;
} }
message DriveHeadingData { message DriveHeadingData {
@ -52,6 +61,8 @@ message GroundCommand {
GroundCommandType type = 2; GroundCommandType type = 2;
oneof data { oneof data {
DriveHeadingData drive_heading = 3; DriveHeadingData drive_heading = 3;
TargetLocation target_location = 4;
config.Config config = 5;
} }
} }

66
main/ugv_comms.cc
View File

@ -14,24 +14,29 @@ namespace ugv {
namespace comms { namespace comms {
static const char *TAG = "ugv_comms"; static const char *TAG = "ugv_comms";
static const TickType_t SEND_PERIOD = pdMS_TO_TICKS(5000);
CommsClass::CommsClass() CommsClass::CommsClass()
: location(), : last_packet_tick(0),
ugv_state(messages::STATE_IDLE),
last_packet_tick(0),
last_packet_rssi(INT32_MIN), last_packet_rssi(INT32_MIN),
last_packet_snr(INT8_MIN) { last_packet_snr(INT8_MIN),
status(),
drive_heading(),
new_target(nullptr),
new_config(nullptr) {
status.set_state(messages::STATE_IDLE);
mutex = xSemaphoreCreateMutex(); mutex = xSemaphoreCreateMutex();
} }
void CommsClass::Init() { void CommsClass::Init() {
esp_err_t ret; esp_err_t ret;
location.set_fix_quality(0); auto *loc = status.mutable_location();
location.set_latitude(43.65); loc->set_fix_quality(0);
location.set_longitude(-116.20); loc->set_latitude(43.65);
location.set_altitude(2730); loc->set_longitude(-116.20);
ugv_state = messages::UGV_State::STATE_IDLE; loc->set_altitude(2730);
status.set_state(messages::UGV_State::STATE_IDLE);
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
sx127x_config_t lora_config = sx127x_config_default(); sx127x_config_t lora_config = sx127x_config_default();
@ -115,9 +120,8 @@ void CommsClass::RunTask() {
using messages::UGV_State; using messages::UGV_State;
using messages::UGV_Status; using messages::UGV_Status;
TickType_t send_period = pdMS_TO_TICKS(2000);
TickType_t current_tick = xTaskGetTickCount(); TickType_t current_tick = xTaskGetTickCount();
TickType_t next_send = current_tick + send_period; TickType_t next_send = current_tick + SEND_PERIOD;
esp_err_t ret; esp_err_t ret;
@ -130,7 +134,6 @@ void CommsClass::RunTask() {
UGV_Message ugv_message; UGV_Message ugv_message;
std::string ugv_message_data; std::string ugv_message_data;
UGV_Status *status = ugv_message.mutable_status();
while (true) { while (true) {
TickType_t delay_ticks = next_send - current_tick; TickType_t delay_ticks = next_send - current_tick;
@ -146,8 +149,8 @@ void CommsClass::RunTask() {
// receiving packet data now // receiving packet data now
rx_len = lora.ReadLn(rx_buf, delay_ticks); rx_len = lora.ReadLn(rx_buf, delay_ticks);
if (rx_len <= 0) { if (rx_len <= 0) {
ESP_LOGI(TAG, "timeout for packet rx"); ESP_LOGV(TAG, "timeout for packet rx");
lora.Flush(); // lora.Flush();
} else { } else {
ESP_LOGV(TAG, "rx data: %.*s", rx_buf.size(), rx_buf.c_str()); ESP_LOGV(TAG, "rx data: %.*s", rx_buf.size(), rx_buf.c_str());
HandlePacket((uint8_t *)rx_buf.c_str(), rx_buf.size()); HandlePacket((uint8_t *)rx_buf.c_str(), rx_buf.size());
@ -162,9 +165,7 @@ void CommsClass::RunTask() {
} }
Lock(); Lock();
status->mutable_location()->CopyFrom(this->location); ugv_message.mutable_status()->CopyFrom(this->status);
status->set_yaw_angle(this->yaw_angle);
status->set_state(this->ugv_state);
Unlock(); Unlock();
ugv_message.SerializeToString(&ugv_message_data); ugv_message.SerializeToString(&ugv_message_data);
ret = SendPacket(ugv_message_data); ret = SendPacket(ugv_message_data);
@ -175,7 +176,7 @@ void CommsClass::RunTask() {
} }
current_tick = xTaskGetTickCount(); current_tick = xTaskGetTickCount();
next_send = current_tick + send_period; next_send = current_tick + SEND_PERIOD;
} }
} }
@ -237,24 +238,43 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
void CommsClass::HandleCommand(const messages::GroundCommand &command) { void CommsClass::HandleCommand(const messages::GroundCommand &command) {
ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type()); ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
using messages::UGV_State;
xSemaphoreTake(mutex, pdMS_TO_TICKS(10)); xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
switch (command.type()) { switch (command.type()) {
case messages::CMD_DISABLE: ugv_state = messages::STATE_IDLE; break; case messages::CMD_DISABLE: status.set_state(UGV_State::STATE_IDLE); break;
case messages::CMD_DRIVE_TO_TARGET: case messages::CMD_DRIVE_TO_TARGET:
ugv_state = messages::STATE_AQUIRING; status.set_state(UGV_State::STATE_AQUIRING);
break; break;
case messages::CMD_TEST: ugv_state = messages::STATE_TEST; break; case messages::CMD_TEST: status.set_state(UGV_State::STATE_TEST); break;
case messages::CMD_DRIVE_HEADING: { case messages::CMD_DRIVE_HEADING: {
if (command.has_drive_heading()) { if (command.has_drive_heading()) {
this->ugv_state = messages::STATE_DRIVE_HEADING; status.set_state(UGV_State::STATE_DRIVE_HEADING);
this->drive_heading = command.drive_heading(); this->drive_heading = command.drive_heading();
} else { } else {
this->ugv_state = messages::STATE_IDLE; status.set_state(UGV_State::STATE_IDLE);
ESP_LOGE(TAG, "invalid CMD_DRIVE_HEADING"); ESP_LOGE(TAG, "invalid CMD_DRIVE_HEADING");
} }
break; break;
} }
case messages::CMD_SET_TARGET: {
if (command.has_target_location()) {
if (!new_target) {
new_target = new messages::TargetLocation();
}
new_target->CopyFrom(command.target_location());
}
break;
}
case messages::CMD_SET_CONFIG: {
if (command.has_config()) {
if (!new_config) {
new_config = new config::Config();
}
new_config->CopyFrom(command.config());
}
break;
}
default: default:
ESP_LOGW(TAG, "unhandled command type: %d", command.type()); ESP_LOGW(TAG, "unhandled command type: %d", command.type());
xSemaphoreGive(mutex); xSemaphoreGive(mutex);

18
main/ugv_comms.hh
View File

@ -15,7 +15,8 @@
namespace ugv { namespace ugv {
namespace comms { namespace comms {
namespace messages = uas::ugv::messages; namespace messages = ugv::messages;
namespace config = ugv::config;
class CommsClass { class CommsClass {
public: public:
@ -33,14 +34,15 @@ class CommsClass {
uint8_t ReadLnaGain(); uint8_t ReadLnaGain();
public: public:
SemaphoreHandle_t mutex; SemaphoreHandle_t mutex;
messages::Location location; TickType_t last_packet_tick;
float yaw_angle; int32_t last_packet_rssi;
messages::UGV_State ugv_state; int8_t last_packet_snr;
TickType_t last_packet_tick;
int32_t last_packet_rssi; messages::UGV_Status status;
int8_t last_packet_snr;
messages::DriveHeadingData drive_heading; messages::DriveHeadingData drive_heading;
messages::TargetLocation* new_target;
config::Config* new_config;
private: private:
#ifdef COMMS_SX127X #ifdef COMMS_SX127X

177
main/ugv_main.cc
View File

@ -1,13 +1,14 @@
#include <esp_log.h> #include <esp_log.h>
#include <esp_timer.h> #include <esp_timer.h>
#include <math.h>
#include "MadgwickAHRS.h" #include "MadgwickAHRS.h"
#include "config.pb.h"
#include "i2c_mutex.h" #include "i2c_mutex.h"
#include "ugv_comms.hh" #include "ugv_comms.hh"
#include "ugv_display.hh" #include "ugv_display.hh"
#include "ugv_io.hh" #include "ugv_io.hh"
#include <math.h>
namespace ugv { namespace ugv {
using ugv::comms::CommsClass; using ugv::comms::CommsClass;
@ -29,14 +30,6 @@ 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 ANGLE_P = 0.02;
static const float ANGLE_I = 0.000005;
static const float ANGLE_D = -0.01;
static const float MAX_ANGLE_POWER = 0.3;
static const float MAX_ANGLE_I_ERROR = 15.0;
static const float MIN_DIST = 10.0;
extern "C" void OnTimeout(void *arg); extern "C" void OnTimeout(void *arg);
void UpdateLocationFromGPS(comms::messages::Location &location, void UpdateLocationFromGPS(comms::messages::Location &location,
@ -52,9 +45,14 @@ struct LatLong {
float latitude; float latitude;
float longitude; float longitude;
inline LatLong() : LatLong(0., 0.) {}
inline LatLong(double latitude_, double longitude_) inline LatLong(double latitude_, double longitude_)
: latitude(latitude_), longitude(longitude_) {} : latitude(latitude_), longitude(longitude_) {}
inline LatLong(const comms::messages::TargetLocation &loc)
: latitude(loc.latitude()), longitude(loc.longitude()) {}
/** /**
* Return distance from this LatLong to target, in meters * Return distance from this LatLong to target, in meters
*/ */
@ -93,7 +91,23 @@ struct LatLong {
class PIDController { class PIDController {
public: public:
explicit PIDController(float dt, float kp, float ki = 0., float kd = 0.); explicit PIDController(float dt, float kp = 0., float ki = 0., float kd = 0.);
void DeltaT(float dt) {
dt_ = dt;
Reset();
}
float DeltaT() { return dt_; }
float GetP() { return kp_; }
float GetI() { return ki_; }
float GetD() { return kd_; }
void SetPID(float kp, float ki, float kd) {
kp_ = kp;
ki_ = ki;
kd_ = kd;
}
void MaxOutput(float max_output) { max_output_ = max_output; } void MaxOutput(float max_output) { max_output_ = max_output; }
float MaxOutput() const { return max_output_; } float MaxOutput() const { return max_output_; }
@ -183,17 +197,18 @@ float PIDController::Update() {
} }
float error = Error(); float error = Error();
output_ += kp_ * error;
if (fabsf(error) > max_i_error_) { if (fabsf(error) > max_i_error_) {
integral_ = 0.; integral_ = 0.;
} else { } else {
integral_ += error * dt_; integral_ += error * dt_;
output_ += ki_ * integral_;
} }
output_ += kp_ * error;
output_ += ki_ * integral_;
if (!isnan(last_error_)) { if (!isnan(last_error_)) {
output_ += kd_ * (error - last_error_); output_ += kd_ * (error - last_error_);
} }
output_ = clamp_mag(output_, max_output_); output_ = clamp_mag(output_, max_output_);
last_error_ = error; last_error_ = error;
@ -217,24 +232,49 @@ struct State {
DisplayClass * display; DisplayClass * display;
esp_timer_handle_t timer_handle; esp_timer_handle_t timer_handle;
io::Inputs inputs; io::Inputs inputs_;
io::Outputs outputs; io::Outputs outputs_;
int64_t last_print; int64_t last_print_;
Madgwick ahrs_; Madgwick ahrs_;
LatLong target; LatLong target_;
PIDController angle_controller_; PIDController angle_controller_;
config::Config conf_;
State() State() : angle_controller_(LOOP_PERIOD_S) {
: target{34.069022, -118.443067}, SetTarget({34.069022, -118.443067});
angle_controller_(LOOP_PERIOD_S, ANGLE_P, ANGLE_I, ANGLE_D) {
angle_controller_.MaxOutput(MAX_ANGLE_POWER);
angle_controller_.MaxIError(MAX_ANGLE_I_ERROR);
comms = new CommsClass(); comms = new CommsClass();
io = new IOClass(); io = new IOClass();
display = new DisplayClass(comms); display = new DisplayClass(comms);
SetConfig(DefaultConfig());
} }
static config::Config DefaultConfig() {
config::Config c;
auto *apid = c.mutable_angle_pid();
apid->set_kp(0.10);
apid->set_ki(0.0);
apid->set_kd(0.4);
apid->set_max_output(0.5);
apid->set_max_i_error(15.0);
c.set_min_target_dist(10.0);
c.set_min_flip_pitch(90.0);
return c;
}
void SetConfig(const config::Config &conf) {
auto &apid = conf.angle_pid();
angle_controller_.SetPID(apid.kp(), apid.ki(), apid.kd());
angle_controller_.MaxOutput(apid.max_output());
angle_controller_.MaxIError(apid.max_i_error());
conf_ = conf;
}
void SetTarget(LatLong target) { target_ = target; }
void Init() { void Init() {
esp_timer_init(); esp_timer_init();
i2c_mutex = xSemaphoreCreateMutex(); i2c_mutex = xSemaphoreCreateMutex();
@ -252,47 +292,60 @@ struct State {
timer_args.name = "ugv_main_loop"; timer_args.name = "ugv_main_loop";
esp_timer_create(&timer_args, &this->timer_handle); esp_timer_create(&timer_args, &this->timer_handle);
esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US); esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US);
last_print = 0; last_print_ = 0;
} }
void OnTick() { void OnTick() {
ESP_LOGV(TAG, "OnTick"); ESP_LOGV(TAG, "OnTick");
int64_t time_us = esp_timer_get_time(); int64_t time_us = esp_timer_get_time();
float time_s = ((float)time_us) / 1e6; // float time_s = ((float)time_us) / 1e6;
io->ReadInputs(inputs); io->ReadInputs(inputs_);
{ {
io::Vec3f &g = inputs.mpu.gyro_rate, &a = inputs.mpu.accel, io::Vec3f &g = inputs_.mpu.gyro_rate, &a = inputs_.mpu.accel,
&m = inputs.mpu.mag; &m = inputs_.mpu.mag;
ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z); ahrs_.update(g.x, g.y, g.z, a.x, a.y, a.z, m.x, m.y, m.z);
} }
if (time_us >= last_print + 500 * 1000) { // 1s if (time_us >= last_print_ + 500 * 1000) { // 1s
ESP_LOGD(TAG, auto &mpu = inputs_.mpu;
"inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)", ESP_LOGD(
inputs.mpu.accel.x, inputs.mpu.accel.y, inputs.mpu.accel.z, TAG, "inputs: acc=(%f, %f, %f) gyro=(%f, %f, %f) mag=(%f, %f, %f)",
inputs.mpu.gyro_rate.x, inputs.mpu.gyro_rate.y, mpu.accel.x, mpu.accel.y, mpu.accel.z, mpu.gyro_rate.x,
inputs.mpu.gyro_rate.z, inputs.mpu.mag.x, inputs.mpu.mag.y, mpu.gyro_rate.y, mpu.gyro_rate.z, mpu.mag.x, mpu.mag.y, mpu.mag.z);
inputs.mpu.mag.z);
ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(), ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
ahrs_.getPitch(), ahrs_.getRoll()); ahrs_.getPitch(), ahrs_.getRoll());
ESP_LOGD(TAG, "PID: error: %f", angle_controller_.Error()); ESP_LOGD(TAG, "PID: error: %f", angle_controller_.Error());
last_print = time_us; last_print_ = time_us;
} }
comms->Lock(); comms->Lock();
UpdateLocationFromGPS(comms->location, inputs.gps); UpdateLocationFromGPS(*(comms->status.mutable_location()), inputs_.gps);
comms->yaw_angle = ahrs_.getYaw(); comms->status.set_yaw_angle(ahrs_.getYaw());
UGV_State ugv_state = comms->ugv_state; UGV_State ugv_state = comms->status.state();
if (comms->new_target) {
SetTarget(*comms->new_target);
ESP_LOGI(TAG, "Updating target to (%f, %f)", target_.latitude,
target_.longitude);
delete comms->new_target;
comms->new_target = nullptr;
}
if (comms->new_config) {
ESP_LOGI(TAG, "Updating config");
SetConfig(*comms->new_config);
delete comms->new_config;
comms->new_config = nullptr;
}
comms->Unlock(); comms->Unlock();
UGV_State next_state = ugv_state; UGV_State next_state = ugv_state;
angle_controller_.Input(ahrs_.getYaw()); angle_controller_.Input(ahrs_.getYaw());
float drive_power = 0.; float drive_power = 0.;
outputs.left_motor = 0.0; outputs_.left_motor = 0.0;
outputs.right_motor = 0.0; outputs_.right_motor = 0.0;
float pitch = ahrs_.getPitch(); float pitch = ahrs_.getPitch();
bool is_upside_down = (pitch > 90.) || (pitch < -90.); auto min_flip_pitch = conf_.min_flip_pitch();
bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch);
switch (ugv_state) { switch (ugv_state) {
default: default:
@ -307,9 +360,9 @@ struct State {
} }
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;
bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000); bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000);
bool not_invalid = inputs.gps.fix_quality != io::GPS_FIX_INVALID; bool not_invalid = inputs_.gps.fix_quality != io::GPS_FIX_INVALID;
if (not_old && not_invalid) { if (not_old && not_invalid) {
next_state = UGV_State::STATE_TURNING; next_state = UGV_State::STATE_TURNING;
} }
@ -317,8 +370,8 @@ struct State {
} }
case UGV_State::STATE_FLIPPING: { case UGV_State::STATE_FLIPPING: {
angle_controller_.Disable(); angle_controller_.Disable();
outputs.left_motor = -1.0; outputs_.left_motor = -1.0;
outputs.right_motor = -1.0; outputs_.right_motor = -1.0;
if (!is_upside_down) { if (!is_upside_down) {
next_state = UGV_State::STATE_AQUIRING; next_state = UGV_State::STATE_AQUIRING;
break; break;
@ -330,13 +383,13 @@ struct State {
next_state = UGV_State::STATE_FLIPPING; next_state = UGV_State::STATE_FLIPPING;
break; 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;
} }
LatLong current_pos = {inputs.gps.latitude, inputs.gps.longitude}; LatLong current_pos = {inputs_.gps.latitude, inputs_.gps.longitude};
float tgt_bearing = current_pos.bearing_toward(target); float tgt_bearing = current_pos.bearing_toward(target_);
angle_controller_.Enable(); angle_controller_.Enable();
angle_controller_.Setpoint(tgt_bearing); angle_controller_.Setpoint(tgt_bearing);
@ -350,21 +403,21 @@ struct State {
next_state = UGV_State::STATE_FLIPPING; next_state = UGV_State::STATE_FLIPPING;
break; 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;
} }
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_);
if (tgt_dist <= MIN_DIST) { if (tgt_dist <= conf_.min_target_dist()) {
ESP_LOGI(TAG, "Finished driving to target"); ESP_LOGI(TAG, "Finished driving to target");
next_state = UGV_State::STATE_FINISHED; next_state = UGV_State::STATE_FINISHED;
break; break;
} }
float tgt_bearing = current_pos.bearing_toward(target); float tgt_bearing = current_pos.bearing_toward(target_);
angle_controller_.Enable(); angle_controller_.Enable();
angle_controller_.Setpoint(tgt_bearing); angle_controller_.Setpoint(tgt_bearing);
break; break;
@ -386,15 +439,15 @@ struct State {
} }
if (angle_controller_.Enabled()) { if (angle_controller_.Enabled()) {
float angle_pwr = angle_controller_.Update(); float angle_pwr = angle_controller_.Update();
outputs.left_motor = drive_power - angle_pwr; outputs_.left_motor = drive_power - angle_pwr;
outputs.right_motor = drive_power + angle_pwr; outputs_.right_motor = drive_power + angle_pwr;
} }
io->WriteOutputs(outputs); io->WriteOutputs(outputs_);
comms->Lock(); comms->Lock();
comms->ugv_state = next_state; comms->status.set_state(next_state);
comms->Unlock(); comms->Unlock();
} }
}; };