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

lots of work on driving and fixing stuff

master
Alex Mikhalev 6 years ago
parent
35d3f6b9c7
commit
7226d8112e
6 changed files with 313 additions and 76 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. 98
      e32_client/messages_pb2.py
  2. 17
      e32_client/ugv.py
  3. 12
      main/messages.proto
  4. 19
      main/ugv_comms.cc
  5. 1
      main/ugv_comms.hh
  6. 218
      main/ugv_main.cc

98
e32_client/messages_pb2.py
Unescape View File

@ -21,7 +21,7 @@ DESCRIPTOR = _descriptor.FileDescriptor(
package='uas.ugv.messages', package='uas.ugv.messages',
syntax='proto3', syntax='proto3',
serialized_options=_b('H\003'), 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( _UGV_STATE = _descriptor.EnumDescriptor(
@ -50,11 +50,23 @@ _UGV_STATE = _descriptor.EnumDescriptor(
name='STATE_TEST', index=4, number=4, name='STATE_TEST', index=4, number=4,
serialized_options=None, serialized_options=None,
type=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, containing_type=None,
serialized_options=None, serialized_options=None,
serialized_start=515, serialized_start=638,
serialized_end=617, serialized_end=804,
) )
_sym_db.RegisterEnumDescriptor(_UGV_STATE) _sym_db.RegisterEnumDescriptor(_UGV_STATE)
@ -77,11 +89,15 @@ _GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor(
name='CMD_TEST', index=2, number=2, name='CMD_TEST', index=2, number=2,
serialized_options=None, serialized_options=None,
type=None), type=None),
_descriptor.EnumValueDescriptor(
name='CMD_DRIVE_HEADING', index=3, number=3,
serialized_options=None,
type=None),
], ],
containing_type=None, containing_type=None,
serialized_options=None, serialized_options=None,
serialized_start=619, serialized_start=806,
serialized_end=694, serialized_end=904,
) )
_sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE) _sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE)
@ -91,9 +107,13 @@ STATE_AQUIRING = 1
STATE_DRIVING = 2 STATE_DRIVING = 2
STATE_FINISHED = 3 STATE_FINISHED = 3
STATE_TEST = 4 STATE_TEST = 4
STATE_FLIPPING = 5
STATE_TURNING = 6
STATE_DRIVE_HEADING = 7
CMD_DISABLE = 0 CMD_DISABLE = 0
CMD_DRIVE_TO_TARGET = 1 CMD_DRIVE_TO_TARGET = 1
CMD_TEST = 2 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( _GROUNDCOMMAND = _descriptor.Descriptor(
name='GroundCommand', name='GroundCommand',
full_name='uas.ugv.messages.GroundCommand', full_name='uas.ugv.messages.GroundCommand',
@ -256,6 +314,13 @@ _GROUNDCOMMAND = _descriptor.Descriptor(
message_type=None, enum_type=None, containing_type=None, message_type=None, enum_type=None, containing_type=None,
is_extension=False, extension_scope=None, is_extension=False, extension_scope=None,
serialized_options=None, file=DESCRIPTOR), 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=[ extensions=[
], ],
@ -267,9 +332,12 @@ _GROUNDCOMMAND = _descriptor.Descriptor(
syntax='proto3', syntax='proto3',
extension_ranges=[], extension_ranges=[],
oneofs=[ oneofs=[
_descriptor.OneofDescriptor(
name='data', full_name='uas.ugv.messages.GroundCommand.data',
index=0, containing_type=None, fields=[]),
], ],
serialized_start=348, serialized_start=401,
serialized_end=426, serialized_end=548,
) )
@ -302,8 +370,8 @@ _GROUNDMESSAGE = _descriptor.Descriptor(
name='ground_message', full_name='uas.ugv.messages.GroundMessage.ground_message', name='ground_message', full_name='uas.ugv.messages.GroundMessage.ground_message',
index=0, containing_type=None, fields=[]), index=0, containing_type=None, fields=[]),
], ],
serialized_start=428, serialized_start=550,
serialized_end=513, serialized_end=635,
) )
_UGV_STATUS.fields_by_name['state'].enum_type = _UGV_STATE _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'])
_UGV_MESSAGE.fields_by_name['command_ack'].containing_oneof = _UGV_MESSAGE.oneofs_by_name['ugv_message'] _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['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.fields_by_name['command'].message_type = _GROUNDCOMMAND
_GROUNDMESSAGE.oneofs_by_name['ground_message'].fields.append( _GROUNDMESSAGE.oneofs_by_name['ground_message'].fields.append(
_GROUNDMESSAGE.fields_by_name['command']) _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['Location'] = _LOCATION
DESCRIPTOR.message_types_by_name['UGV_Status'] = _UGV_STATUS DESCRIPTOR.message_types_by_name['UGV_Status'] = _UGV_STATUS
DESCRIPTOR.message_types_by_name['UGV_Message'] = _UGV_MESSAGE 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['GroundCommand'] = _GROUNDCOMMAND
DESCRIPTOR.message_types_by_name['GroundMessage'] = _GROUNDMESSAGE DESCRIPTOR.message_types_by_name['GroundMessage'] = _GROUNDMESSAGE
DESCRIPTOR.enum_types_by_name['UGV_State'] = _UGV_STATE 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) _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( GroundCommand = _reflection.GeneratedProtocolMessageType('GroundCommand', (_message.Message,), dict(
DESCRIPTOR = _GROUNDCOMMAND, DESCRIPTOR = _GROUNDCOMMAND,
__module__ = 'messages_pb2' __module__ = 'messages_pb2'

17
e32_client/ugv.py
Unescape View File

@ -35,13 +35,16 @@ class UGVComms(E32):
data = msg.SerializeToString() data = msg.SerializeToString()
self.write_base64(data) 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 cmdid = self.next_command_id
self.next_command_id += 1 self.next_command_id += 1
gmsg = messages.GroundMessage() gmsg = messages.GroundMessage()
if type(command) is int:
gmsg.command.type = command
else:
gmsg.command.CopyFrom(command)
gmsg.command.id = cmdid gmsg.command.id = cmdid
gmsg.command.type = cmd_type
self.write_message(gmsg) self.write_message(gmsg)
last_write_time = time.time() last_write_time = time.time()
if not retry: if not retry:
@ -117,9 +120,13 @@ def main():
time.sleep(0.2) time.sleep(0.2)
try: try:
while True: while True:
if ugv.last_status is None or ugv.last_status.state is not messages.STATE_TEST: if ugv.last_status is None or ugv.last_status.state is not messages.STATE_DRIVE_HEADING:
ugv.write_command(messages.CMD_TEST) cmd = messages.GroundCommand()
time.sleep(2.) 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: except KeyboardInterrupt:
ugv.write_command(messages.CMD_DISABLE) ugv.write_command(messages.CMD_DISABLE)
print("exiting...") print("exiting...")

12
main/messages.proto
Unescape View File

@ -10,6 +10,9 @@ enum UGV_State {
STATE_DRIVING = 2; STATE_DRIVING = 2;
STATE_FINISHED = 3; STATE_FINISHED = 3;
STATE_TEST = 4; STATE_TEST = 4;
STATE_FLIPPING = 5;
STATE_TURNING = 6;
STATE_DRIVE_HEADING = 7;
} }
message Location { message Location {
@ -36,11 +39,20 @@ enum GroundCommandType {
CMD_DISABLE = 0; CMD_DISABLE = 0;
CMD_DRIVE_TO_TARGET = 1; CMD_DRIVE_TO_TARGET = 1;
CMD_TEST = 2; CMD_TEST = 2;
CMD_DRIVE_HEADING = 3;
}
message DriveHeadingData {
float heading = 1;
float power = 2;
} }
message GroundCommand { message GroundCommand {
uint32 id = 1; uint32 id = 1;
GroundCommandType type = 2; GroundCommandType type = 2;
oneof data {
DriveHeadingData drive_heading = 3;
}
} }
message GroundMessage { message GroundMessage {

19
main/ugv_comms.cc
Unescape View File

@ -144,9 +144,9 @@ void CommsClass::RunTask() {
} }
#else #else
// receiving packet data now // 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) { if (rx_len <= 0) {
ESP_LOGE(TAG, "timeout for packet rx"); ESP_LOGI(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());
@ -200,6 +200,7 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
xSemaphoreTake(mutex, pdMS_TO_TICKS(10)); xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
last_packet_tick = xTaskGetTickCount(); last_packet_tick = xTaskGetTickCount();
xSemaphoreGive(mutex); xSemaphoreGive(mutex);
ESP_LOGI(TAG, "rx base64 message: %.*s", data_size, data);
int ret; int ret;
size_t decoded_size = (data_size * 4 + 2) / 3; 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, ret = mbedtls_base64_decode(decoded, decoded_size, &decoded_len, data,
data_size); data_size);
if (ret != 0) { if (ret != 0) {
ESP_LOGE(TAG, "error with base64 decode: %d", ret); ESP_LOGE(TAG, "base64 decode error: %d", ret);
delete[] decoded; delete[] decoded;
return; return;
} }
@ -244,6 +245,16 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) {
ugv_state = messages::STATE_AQUIRING; ugv_state = messages::STATE_AQUIRING;
break; break;
case messages::CMD_TEST: ugv_state = messages::STATE_TEST; 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: default:
ESP_LOGW(TAG, "unhandled command type: %d", command.type()); ESP_LOGW(TAG, "unhandled command type: %d", command.type());
xSemaphoreGive(mutex); 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); ESP_LOGE(TAG, "SendPacket size too long: %d", size);
return ESP_ERR_INVALID_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]; uint8_t *encoded = new uint8_t[encoded_size];
size_t encoded_len; size_t encoded_len;
int ret = int ret =

1
main/ugv_comms.hh
Unescape View File

@ -40,6 +40,7 @@ class CommsClass {
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::DriveHeadingData drive_heading;
private: private:
#ifdef COMMS_SX127X #ifdef COMMS_SX127X

218
main/ugv_main.cc
Unescape View File

@ -21,35 +21,29 @@ SemaphoreHandle_t i2c_mutex;
} }
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
constexpr float LOOP_PERIOD_S = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
static const float PI = static const float PI =
3.1415926535897932384626433832795028841971693993751058209749445923078164062; 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; 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.05; 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; static const float MIN_DIST = 10.0;
float clamp_mag(float x, float max_mag) { extern "C" void OnTimeout(void *arg);
if (x > max_mag)
return max_mag; void UpdateLocationFromGPS(comms::messages::Location &location,
else if (x < -max_mag) const io::GpsData & gps_data) {
return -max_mag; location.set_fix_quality(gps_data.fix_quality);
else location.set_latitude(gps_data.latitude);
return x; location.set_longitude(gps_data.longitude);
location.set_altitude(gps_data.altitude);
} }
struct LatLong { 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 { struct State {
public: public:
CommsClass * comms; CommsClass * comms;
IOClass * io; IOClass * io;
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_;
State() : target{34.069022, -118.443067} { State()
: target{34.069022, -118.443067},
angle_controller_(LOOP_PERIOD_S, ANGLE_P) {
angle_controller_.MaxOutput(MAX_ANGLE_POWER);
comms = new CommsClass(); comms = new CommsClass();
io = new IOClass(); io = new IOClass();
display = new DisplayClass(comms); display = new DisplayClass(comms);
@ -118,8 +227,7 @@ struct State {
esp_timer_init(); esp_timer_init();
i2c_mutex = xSemaphoreCreateMutex(); i2c_mutex = xSemaphoreCreateMutex();
ahrs_.begin(1000000.f / ahrs_.begin(LOOP_PERIOD_S); // rough sample frequency
static_cast<float>(LOOP_PERIOD_US)); // rough sample frequency
io->Init(); io->Init();
comms->Init(); comms->Init();
@ -164,23 +272,44 @@ struct State {
comms->Unlock(); comms->Unlock();
UGV_State next_state = ugv_state; 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) { switch (ugv_state) {
default: default:
ESP_LOGW(TAG, "unhandled state: %d", ugv_state); ESP_LOGW(TAG, "unhandled state: %d", ugv_state);
// fall through // fall through
case UGV_State::STATE_IDLE: case UGV_State::STATE_IDLE:
case UGV_State::STATE_FINISHED: case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break;
outputs.left_motor = 0.0;
outputs.right_motor = 0.0;
break;
case UGV_State::STATE_AQUIRING: { case UGV_State::STATE_AQUIRING: {
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;
outputs.left_motor = 0.0;
outputs.right_motor = 0.0;
if (not_old && not_invalid) { 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; next_state = UGV_State::STATE_DRIVING;
} }
break; break;
@ -201,15 +330,8 @@ struct State {
} }
float tgt_bearing = current_pos.bearing_toward(target); float tgt_bearing = current_pos.bearing_toward(target);
float cur_bearing = ahrs_.getYaw(); angle_controller_.Enable();
float angle_delta = tgt_bearing - cur_bearing; angle_controller_.Setpoint(tgt_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;
break; break;
} }
case UGV_State::STATE_TEST: case UGV_State::STATE_TEST:
@ -217,19 +339,23 @@ struct State {
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 #else
float tgt_bearing = 90.0; angle_controller_.Enable();
float cur_bearing = ahrs_.getYaw(); angle_controller_.Setpoint(90.0);
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 #endif
break; 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); io->WriteOutputs(outputs);
comms->Lock(); comms->Lock();

Write Preview
Loading…
Cancel
Save