Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

New radio protocol using protobuf #127

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@

# build directories
/build
cmake-build-*
build-*
*/build
/gmon.out
*.swp
Expand Down
3 changes: 3 additions & 0 deletions .gitmodules
Original file line number Diff line number Diff line change
Expand Up @@ -7,3 +7,6 @@
[submodule "control/mtrain"]
path = control/mtrain
url = https://github.com/RoboJackets/mtrain-firmware.git
[submodule "control/external/nanopb"]
path = control/external/nanopb
url = https://github.com/nanopb/nanopb
6 changes: 6 additions & 0 deletions control/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,21 @@ project(
LANGUAGES ASM C CXX
)

set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/external/nanopb/extra)
find_package(Nanopb REQUIRED)

add_subdirectory(mtrain)
add_subdirectory(external)
add_subdirectory(src/robocup/proto)

file(GLOB_RECURSE SRC "src/*.cpp")

add_library(control-lib
${SRC}
)

target_link_libraries(control-lib robocup_protos)

## Shouldnt need explicit inclusion since mtrain dev relies on these
target_include_directories(control-lib PUBLIC
include
Expand Down
1 change: 1 addition & 0 deletions control/external/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1 +1,2 @@
add_subdirectory(robocup-fshare)
add_subdirectory(nanopb)
1 change: 1 addition & 0 deletions control/external/nanopb
Submodule nanopb added at ba0628
44 changes: 33 additions & 11 deletions control/include/robocup/MicroPackets.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

#include "cstdint"
#include <array>
#include <vector>

// Micropackets are shared memory locations for values
// where only the latest matters. There is no guarantee that
Expand Down Expand Up @@ -137,8 +138,12 @@ struct KickerCommand {
bool isValid = false; /**< Stores whether given data is valid */
uint32_t lastUpdate; /**< Time at which MotionCommand was last updated (milliseconds) */

enum ShootMode { KICK = 0, CHIP = 1 };
enum TriggerMode { OFF = 0, IMMEDIATE = 1, ON_BREAK_BEAM = 2, INVALID = 3 };
enum ShootMode {
KICK = 0, CHIP = 1
};
enum TriggerMode {
OFF = 0, IMMEDIATE = 1, ON_BREAK_BEAM = 2, INVALID = 3
};

ShootMode shootMode; /**< 1-bit encoding of whether the kicker should perform a ground kicker or a chip */
TriggerMode triggerMode; /**< 2-bit encoding of when kicker should kick (none, immediately, on breakbeam, cancel) */
Expand Down Expand Up @@ -174,20 +179,37 @@ struct KickerInfo {
* Passed from any module to @ref RadioModule
*/
struct DebugFrame {
uint64_t ticks;
uint32_t frame_time_ms;

float gyro_z;
float accel_x;
float accel_y;
std::array<float, 4> wheel_speeds_radps;
std::array<float, 4> motor_outputs_pwm;

float filtered_velocity[3];
std::array<float, 3> filtered_pose;
std::array<float, 3> filtered_velocity;

float motor_outputs[4];
bool has_last_motion_command;
uint32_t last_motion_command_sequence_number;

float encDeltas[4]; // encoder changes since last packet
float gyro_z_radps;
std::array<float, 3> measured_acceleration;
};

struct LogMessage {
enum class LogLevel {
LEVEL_FATAL = 0,
LEVEL_ERROR = 1,
LEVEL_WARNING = 2,
LEVEL_INFO = 3,
LEVEL_VERBOSE = 4,
};

uint32_t timestamp_ms;
LogLevel level;
std::array<char, 16> subsystem;
std::array<char, 256> message;
};

struct DebugInfo {
int num_debug_frames = 0;
std::array<DebugFrame, 8> debug_frames;
std::vector<DebugFrame> debug_frames;
std::vector<LogMessage> log_messages;
};
1 change: 1 addition & 0 deletions control/include/robocup/motion-control/RobotController.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ class RobotController {
* Body velocity control gains (1/s, 1/s, 1/s)
*/
Eigen::Matrix<float, numStates, 1> BodyKp;
float WheelKp = 0.0;

/**
* Interval of control calculations (seconds)
Expand Down
87 changes: 46 additions & 41 deletions control/src/robocup/modules/MotionControlModule.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,22 +33,24 @@ MotionControlModule::MotionControlModule(LockedStruct<BatteryVoltage>& batteryVo
}

void MotionControlModule::entry() {
auto motionCommandLock = motionCommand.lock();
auto motorCommandLock = motorCommand.lock();
auto motorFeedbackLock = motorFeedback.lock();
auto imuDataLock = imuData.lock();
auto batteryVoltageLock = batteryVoltage.lock();

DebugFrame frame;
frame.ticks = xTaskGetTickCount();
frame.gyro_z = imuDataLock->omegas[2];
frame.accel_x = imuDataLock->accelerations[0];
frame.accel_y = imuDataLock->accelerations[1];
auto motion_command = motionCommand.lock().value();
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changes in this file do not completely fill in the new debug frame format. They were mostly intended to fix the fact that we were holding waaay too many locks throughout this whole function.

Later work will involve actually populating the new data (and providing logging facilities 😄 )

auto motor_feedback = motorFeedback.lock().value();
auto imu_data = imuData.lock().value();
auto battery_voltage_data = batteryVoltage.lock().value();

MotorCommand motor_command;

DebugFrame frame{};
frame.frame_time_ms = xTaskGetTickCount();
frame.gyro_z_radps = imu_data.omegas[2];
frame.measured_acceleration[0] = imu_data.accelerations[0];
frame.measured_acceleration[1] = imu_data.accelerations[1];
frame.measured_acceleration[2] = imu_data.accelerations[2];

// No radio comm in a little while. Return and die.
if (!motionCommandLock->isValid || !isRecentUpdate(motionCommandLock->lastUpdate)) {
motorCommandLock->isValid = false;
motorCommandLock->lastUpdate = HAL_GetTick();
if (!motion_command.isValid || !isRecentUpdate(motion_command.lastUpdate)) {
motor_command.isValid = false;
motor_command.lastUpdate = HAL_GetTick();
}

// Fill data from shared mem
Expand All @@ -57,43 +59,42 @@ void MotionControlModule::entry() {
measurements << 0, 0, 0, 0, 0;
currentWheels << 0, 0, 0, 0;

if (motorFeedbackLock->isValid && isRecentUpdate(motorFeedbackLock->lastUpdate)) {
if (motor_feedback.isValid && isRecentUpdate(motor_feedback.lastUpdate)) {
for (int i = 0; i < 4; i++) {
if (!isnan(measurements(i,0))) {
measurements(i, 0) = motorFeedbackLock->encoders[i];
currentWheels(i, 0) = motorFeedbackLock->encoders[i];
measurements(i, 0) = motor_feedback.encoders[i];
currentWheels(i, 0) = motor_feedback.encoders[i];
} else {
measurements(i, 0) = 0;
currentWheels(i, 0) = 0;
}
}
}

if (imuDataLock->isValid && isRecentUpdate(imuDataLock->lastUpdate)) {
measurements(4, 0) = imuDataLock->omegas[2]; // Z gyro
if (imu_data.isValid && isRecentUpdate(imu_data.lastUpdate)) {
measurements(4, 0) = imu_data.omegas[2]; // Z gyro
}

// Update targets
Eigen::Matrix<float, 3, 1> targetState;
targetState << 0, 0, 0;

if (motionCommandLock->isValid && isRecentUpdate(motionCommandLock->lastUpdate)) {
targetState << motionCommandLock->bodyXVel,
motionCommandLock->bodyYVel,
motionCommandLock->bodyWVel;
if (motion_command.isValid && isRecentUpdate(motion_command.lastUpdate)) {
targetState << motion_command.bodyXVel,
motion_command.bodyYVel,
motion_command.bodyWVel;
}

// Run estimators
robotEstimator.predict(prevCommand);


// Only use the feedback if we have good inputs
// NAN's most likely came from the divide by dt in the fpga
// which was 0, resulting in bad behavior
// Leaving this hear until someone can test, then remove
// this
// - Joe Aug 2019
if (motorFeedbackLock->isValid && // imuData->isValid &&
if (motor_feedback.isValid && // imuData->isValid &&
!isnan(measurements(0,0)) &&
!isnan(measurements(1,0)) &&
!isnan(measurements(2,0)) &&
Expand All @@ -114,7 +115,7 @@ void MotionControlModule::entry() {

// Run controllers
uint8_t dribblerCommand = 0;
dribblerController.calculate(motionCommandLock->dribbler, dribblerCommand);
dribblerController.calculate(motion_command.dribbler, dribblerCommand);

Eigen::Matrix<float, 4, 1> targetWheels;
Eigen::Matrix<float, 4, 1> motorCommands;
Expand All @@ -124,40 +125,44 @@ void MotionControlModule::entry() {

prevCommand = motorCommands;

motorCommandLock->isValid = true;
motorCommandLock->lastUpdate = HAL_GetTick();
motor_command.isValid = true;
motor_command.lastUpdate = HAL_GetTick();

// Good to run motors
// todo Check stall and motor errors?
if (batteryVoltageLock->isValid && !batteryVoltageLock->isCritical) {
if (battery_voltage_data.isValid && !battery_voltage_data.isCritical) {

// set motors to real targets
for (int i = 0; i < 4; i++) {
motorCommandLock->wheels[i] = motorCommands(i, 0);
motor_command.wheels[i] = motorCommands(i, 0);
}
motorCommandLock->dribbler = dribblerCommand;
motor_command.dribbler = dribblerCommand;
} else {

// rip battery
// stop
for (int i = 0; i < 4; i++) {
motorCommandLock->wheels[i] = 0.0f;
motor_command.wheels[i] = 0.0f;
}
motorCommandLock->dribbler = 0;
motor_command.dribbler = 0;
}

for (int i = 0; i < 4; i++) {
frame.motor_outputs[i] = static_cast<int16_t>(motorCommandLock->wheels[i] * 511);
frame.encDeltas[i] = static_cast<int16_t>(currentWheels(i));
frame.motor_outputs_pwm[i] = motor_command.wheels[i];
frame.wheel_speeds_radps[i] = currentWheels(i);
}

{
auto debugInfoLock = debugInfo.lock();
if (debugInfoLock->debug_frames.size() < 16) {
debugInfoLock->debug_frames.push_back(frame);
}
}

#if 0
auto debugInfoLock = debugInfo.lock();
if (debugInfoLock->num_debug_frames < debugInfoLock->debug_frames.size()) {
debugInfoLock->debug_frames[debugInfoLock->num_debug_frames] = frame;
debugInfoLock->num_debug_frames++;
{
auto motorCommandLock = motorCommand.lock();
motorCommandLock.value() = motor_command;
}
#endif
}

bool MotionControlModule::isRecentUpdate(uint32_t lastUpdateTime) {
Expand Down
28 changes: 14 additions & 14 deletions control/src/robocup/modules/RadioModule.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,22 +70,22 @@ void RadioModule::entry() {
link.send(battery, fpga, kicker, id, debug);
}

{
auto motionCommandLock = motionCommand.lock();
auto radioErrorLock = radioError.lock();
auto kickerCommandLock = kickerCommand.lock();
MotionCommand received_motion_command;
KickerCommand received_kicker_command;

// Try read
// Clear buffer of old packets such that we can get the lastest packet
// If you don't do this there is a significant lag of 300ms or more
while (link.receive(kickerCommandLock.value(), motionCommandLock.value())) {
kickerCommandLock->isValid = true;
kickerCommandLock->lastUpdate = HAL_GetTick();

motionCommandLock->isValid = true;
motionCommandLock->lastUpdate = HAL_GetTick();
}
// Try read
// Clear buffer of old packets such that we can get the lastest packet
// If you don't do this there is a significant lag of 300ms or more
while (link.receive(received_kicker_command, received_motion_command)) {}

if (received_motion_command.isValid){
motionCommand.lock().value() = received_motion_command;
}
if (received_kicker_command.isValid){
kickerCommand.lock().value() = received_kicker_command;
}
{
auto radioErrorLock = radioError.lock();
radioErrorLock->isValid = true;
radioErrorLock->lastUpdate = HAL_GetTick();
radioErrorLock->hasConnectionError = link.isRadioConnected();
Expand Down
6 changes: 6 additions & 0 deletions control/src/robocup/proto/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
nanopb_generate_cpp(PROTO_SRCS PROTO_HDRS radio_protocol.proto)
add_library(robocup_protos ${PROTO_SRCS} ${PROTO_HDRS})

target_include_directories(robocup_protos PUBLIC ${CMAKE_CURRENT_BINARY_DIR} ${NANOPB_INCLUDE_DIRS})
set_source_files_properties(${PROTO_SRCS} ${PROTO_HDRS}
PROPERTIES GENERATED TRUE)
Loading