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

send config over rf link

master
Alex Mikhalev 6 years ago
parent
9fe62e38af
commit
31f41c4b3c
6 changed files with 202 additions and 94 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      main/CMakeLists.txt
  2. 20
      main/config.proto
  3. 13
      main/messages.proto
  4. 66
      main/ugv_comms.cc
  5. 10
      main/ugv_comms.hh
  6. 169
      main/ugv_main.cc

2
main/CMakeLists.txt
Unescape 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
Unescape 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
Unescape 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
Unescape 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);

10
main/ugv_comms.hh
Unescape 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:
@ -34,13 +35,14 @@ class CommsClass {
public: public:
SemaphoreHandle_t mutex; SemaphoreHandle_t mutex;
messages::Location location;
float yaw_angle;
messages::UGV_State ugv_state;
TickType_t last_packet_tick; TickType_t last_packet_tick;
int32_t last_packet_rssi; int32_t last_packet_rssi;
int8_t last_packet_snr; int8_t last_packet_snr;
messages::UGV_Status status;
messages::DriveHeadingData drive_heading; messages::DriveHeadingData drive_heading;
messages::TargetLocation* new_target;
config::Config* new_config;
private: private:
#ifdef COMMS_SX127X #ifdef COMMS_SX127X

169
main/ugv_main.cc
Unescape 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_ += kp_ * error;
output_ += ki_ * integral_; 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;
@ -387,14 +440,14 @@ 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();
} }
}; };

Write Preview
Loading…
Cancel
Save