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.

Sign up for GitHub

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

メッセージの再構築 #13

Draft
wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

メッセージの再構築 #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
4 changes: 2 additions & 2 deletions Core/Inc/AcknowledgeMessage.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@

typedef struct AcknowledgeMessageType
{
// timestamp longtime mod 256
uint8_t timestamp;
// timestamp longtime mod 65,536
uint16_t timestamp;
} acknowledge_message_t;

typedef sb::CANMessage<acknowledge_message_t> AcknowledgeMessage;
Expand Down
14 changes: 10 additions & 4 deletions Core/Inc/CommandMessage.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,15 +5,21 @@

enum class Command : uint8_t
{
NO_OPERATION
RESET = 0U,
PING = 1U,
SET_PWM_FREQUENCY_AS_20KHZ = 2U,
SET_PWM_FREQUENCY_AS_4KHZ = 2U,
ENCODER_1_COUNT_RESET = 10U,
ENCODER_2_COUNT_RESET = 11U,
ENCODER_3_COUNT_RESET = 12U,
ENCODER_4_COUNT_RESET = 13U,
};

typedef struct CommandMessageType
{
Command command;
uint32_t value;
// timestamp longtime mod 256
uint8_t timestamp;
// timestamp longtime mod 65,536
uint16_t timestamp;
} command_message_t;

typedef sb::CANMessage<command_message_t> CommandMessage;
Expand Down
1 change: 0 additions & 1 deletion Core/Inc/FeedbackMessage.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@ typedef struct NodeFeedbackMessageType
{
float angle;
float velocity;
float current;
} node_feedback_message_t;

typedef struct FeedbackMessageType
Expand Down
1 change: 0 additions & 1 deletion Core/Inc/MessageID.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@
#define MDC_ID_NODE_RANGE 10

enum class MessageID: uint32_t {
PING,
COMMAND,
ACKNOWLEDGE,
SETTING,
Expand Down
13 changes: 0 additions & 13 deletions Core/Inc/PingMessage.hpp
View file
Open in desktop

This file was deleted.

24 changes: 14 additions & 10 deletions Core/Inc/SettingMessage.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,16 +4,24 @@
#include "Message.hpp"
#include "EncoderType.hpp"

enum class Motor : uint8_t
{
MOTOR1 = 0U,
MOTOR2 = 1U,
MOTOR3 = 2U,
MOTOR4 = 3U
};

enum class OperatorMode : uint8_t
{
NO_OPERATOR,
MD_OPERATOR,
PID_OPERATOR
NO_OPERATOR = 0U,
MD_OPERATOR = 1U,
PID_OPERATOR = 2U
};

typedef struct SettingMessageType
{
uint8_t nodeId;
Motor motorId;
OperatorMode mode;
EncoderType encoderType;
float scale;
Expand All @@ -23,12 +31,8 @@ typedef struct SettingMessageType
float ki;
float kd;
float forward_gain;
// current pid
float cp;
float ci;
float cd;
// timestamp longtime mod 256
uint8_t timestamp;
// timestamp longtime mod 65,536
uint16_t timestamp;
} setting_message_t;

typedef sb::CANMessage<setting_message_t> SettingMessage;
Expand Down
76 changes: 48 additions & 28 deletions Core/Src/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,6 @@
#include "CANSerialBridge.hpp"
// Serial Bridge Message
#include "MessageID.hpp"
#include "PingMessage.hpp"
#include "CommandMessage.hpp"
#include "AcknowledgeMessage.hpp"
#include "SettingMessage.hpp"
Expand Down Expand Up @@ -160,7 +159,6 @@ ACAN2517FD dev_can(dev_spi, get_milliseconds);
ACAN2517FDSettings can_settings(ACAN2517FDSettings::OSC_4MHz, 500UL * 1000UL, DataBitRateFactor::x2);
CANSerialBridge serial(&dev_can);

PingMessage ping_msg;
CommandMessage command_msg;
AcknowledgeMessage acknowledge_msg;
SettingMessage setting_msg;
Expand Down Expand Up @@ -285,16 +283,12 @@ int main(void)
}

// add frame
const static uint32_t PING_ID = resolve_id(device_id, MessageID::PING);
const static uint32_t COMMAND_ID = resolve_id(device_id, MessageID::COMMAND);
const static uint32_t ACKNOWLEDGE_ID = resolve_id(device_id, MessageID::ACKNOWLEDGE);
const static uint32_t SETTING_ID = resolve_id(device_id, MessageID::SETTING);
const static uint32_t TARGET_ID = resolve_id(device_id, MessageID::TARGET);
const static uint32_t FEEDBACK_ID = resolve_id(device_id, MessageID::FEEDBACK);

if (serial.add_frame(PING_ID, &ping_msg) != 0) {
printf("Failed to register PING message.\n\r");
}
if (serial.add_frame(COMMAND_ID, &command_msg) != 0) {
printf("Failed to register COMMAND message.\n\r");
}
Expand Down Expand Up @@ -326,29 +320,49 @@ int main(void)

double ping_tim = (TIM_COUNT_US - last_ping_at) / 1E6;
if (ping_tim > PING_INTERVAL) {
serial.write(PING_ID);
// set ping command
command_msg.data.command = Command::PING;
command_msg.data.timestamp = TIM_COUNT_US / 65536;

serial.write(COMMAND_ID);
last_ping_at = TIM_COUNT_US;
}

double sb_tim = (TIM_COUNT_US - last_sb_at) / 1E6;
if (sb_tim > SB_INTERVAL) {
serial.update();

// ping message
if (ping_msg.was_updated()) {
ping_msg.data.v = device_id;

// update timestamp last received at
last_received_at = TIM_COUNT_US;
// toggle acknowledge
toggleAcknowledge();
}

// command message
if (command_msg.was_updated()) {
// there's no feature...
// feature coming soon...?

if (command_msg.data.command == Command::RESET) {
for (int i = 0; i < 4; ++i) {
// reset operator
operators[i]->reset();
operators[i] = new NoOperator();
// md
md[i]->set(0);
// encoder
encoder[i]->reset_count();
// pid
vel_ctrl[i]->reset();
}
}
else if (command_msg.data.command == Command::ENCODER_1_COUNT_RESET) {
encoder[M1]->reset_count();
vel_ctrl[M1]->reset();
}
else if (command_msg.data.command == Command::ENCODER_2_COUNT_RESET) {
encoder[M2]->reset_count();
vel_ctrl[M2]->reset();
}
else if (command_msg.data.command == Command::ENCODER_3_COUNT_RESET) {
encoder[M3]->reset_count();
vel_ctrl[M3]->reset();
}
else if (command_msg.data.command == Command::ENCODER_4_COUNT_RESET) {
encoder[M4]->reset_count();
vel_ctrl[M4]->reset();
}

// acknowledge
acknowledge_msg.data.timestamp = command_msg.data.timestamp;
Expand All @@ -360,10 +374,17 @@ int main(void)
toggleAcknowledge();
}

if (acknowledge_msg.was_updated()) {
// update timestamp last received at
last_received_at = TIM_COUNT_US;
// toggle acknowledge
toggleAcknowledge();
}

// setting message
if (setting_msg.was_updated()) {
if (setting_msg.data.nodeId < 4) {
uint32_t id = setting_msg.data.nodeId;
if ((uint8_t) setting_msg.data.motorId < 4) {
uint32_t id = (uint8_t) setting_msg.data.motorId;
// reset
operators[id]->reset();

Expand All @@ -389,13 +410,13 @@ int main(void)
}

// gain
p_vel[setting_msg.data.nodeId].kp = setting_msg.data.kp;
p_vel[setting_msg.data.nodeId].ki = setting_msg.data.ki;
p_vel[setting_msg.data.nodeId].kd = setting_msg.data.kd;
p_vel[setting_msg.data.nodeId].forward_gain = setting_msg.data.forward_gain;
p_vel[id].kp = setting_msg.data.kp;
p_vel[id].ki = setting_msg.data.ki;
p_vel[id].kd = setting_msg.data.kd;
p_vel[id].forward_gain = setting_msg.data.forward_gain;

// reset pid
vel_ctrl[setting_msg.data.nodeId]->reset();
vel_ctrl[id]->reset();

// acknowledge
acknowledge_msg.data.timestamp = setting_msg.data.timestamp;
Expand All @@ -421,7 +442,6 @@ int main(void)
for (int i = 0; i < 4; i++) {
feedback_msg.data.node[i].angle = (float) encoder[i]->get_angle();
feedback_msg.data.node[i].velocity = (float) encoder[i]->get_velocity();
feedback_msg.data.node[i].current = (float) current_sensor->get_current(i);
}

// write
Expand Down