Skip to content

Commit

Permalink
Merge pull request #21 from RoboJackets/stable
Browse files Browse the repository at this point in the history 
Merging Stuff from Comp
  • Loading branch information
@AR2100
AR2100 authored Sep 15, 2024
2 parents 4e55f62 + 02a9740 commit 9f5547a
Show file tree
Hide file tree
Showing 16 changed files with 1,051 additions and 223 deletions.
2 changes: 2 additions & 0 deletions lib/Messages/include/Messages.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@ class Messages {

public:

// static bool decode(int messageType, )

static bool decodeStatusLightRequest(uint8_t *buffer, size_t bufferLen, StatusLightCommand &requestMessage) {
pb_istream_t istream = pb_istream_from_buffer(buffer, bufferLen);
return pb_decode(&istream, StatusLightCommand_fields, &requestMessage);
Expand Down
78 changes: 30 additions & 48 deletions lib/SoloCAN/include/SoloCAN.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
#include <Arduino.h>
#include <FlexCAN_T4.h>
#include <cmath>
#include <queue>

namespace solo_can {

Expand All @@ -16,13 +17,22 @@ namespace solo_can {
const int TORQUE_REF_CODE = 0x3004;
const int SPEED_FEEDBACK_CODE = 0x3036;
const int POSITION_FEEDBACK_CODE = 0x3037;
const int QUADRATURE_CURRENT_FEEDBACK_CODE = 0x3034;
const int MOTOR_DIRECTION_CODE = 0x300C;
const int CONTROL_MODE_CODE = 0x3016;

const uint32_t MODE_SPEED = 0;
const uint32_t MODE_TORQUE = 1;
const uint32_t MODE_POSITION = 2;

static float toFloat(uint32_t payload) {
if (payload <= 0x7FFE0000) {
return payload / 131072.0;
} else {
return (0xFFFFFFFF - payload + 0x1) / -131072.0;
}
}

struct CanOpenData {
uint16_t id;
uint8_t type;
Expand Down Expand Up @@ -56,19 +66,21 @@ namespace solo_can {
return data;
}

float toFloat(uint32_t payload) {
if (payload <= 0x7FFE0000) {
return payload / 131072.0;
} else {
return (0xFFFFFFFF - payload + 0x1) / -131072.0;
}
}


template<CAN_DEV_TABLE _bus, FLEXCAN_RXQUEUE_TABLE _rxSize, FLEXCAN_TXQUEUE_TABLE _txSize>
class SoloCan {
public:
SoloCan(FlexCAN_T4<_bus, _rxSize, _txSize> &_can) : can(_can) {}

void processMessageQueue() {
while (!sendQueue.empty() && can.getTXQueueCount() < 12) {
can.write(sendQueue.front());
can.
sendQueue.pop();
}
}

void GetBoardTemperatureCommand(int soloID) {
struct CanOpenData data = (struct CanOpenData){
.id = 0x0600 + soloID,
Expand All @@ -77,7 +89,6 @@ namespace solo_can {
.payload = 0
};
can.write(createMessage(data));
delayMicroseconds(200);
}

void GetSpeedFeedbackCommand(int soloID) {
Expand All @@ -88,7 +99,6 @@ namespace solo_can {
.payload = 0
};
can.write(createMessage(data));
delayMicroseconds(200);
}

void GetPositionFeedbackCommand(int soloID) {
Expand All @@ -99,66 +109,39 @@ namespace solo_can {
.payload = 0
};
can.write(createMessage(data));
delayMicroseconds(200);
}

void GetCurrentDrawCommand(int soloID) {
struct CanOpenData data = {
.id = (uint16_t)(0x0600 + soloID),
.type = SDO_READ_COMMAND,
.code = QUADRATURE_CURRENT_FEEDBACK_CODE,
.payload = 0
};
can.write(createMessage(data));
}

void SetSpeedReferenceCommand(int soloID, int speedRef, bool isReversed) {
uint32_t dir = (uint32_t)(speedRef < 0 ? 1 : 0);
if (isReversed) dir ^= 1;
uint32_t speedMag = (uint32_t)abs(speedRef);
struct CanOpenData data;

// Serial.print("Speed ref: ");
// Serial.println(speedMag);

// // set control mode
// if (speedRef == 0 && (abs(speedFeedback) <= 200)) {
// data = (struct CanOpenData) {
// .id = (uint16_t)(0x0600 + soloID),
// .type = SDO_WRITE_COMMAND,
// .code = CONTROL_MODE_CODE,
// .payload = MODE_TORQUE
// };

// } else {
// data = (struct CanOpenData) {
// .id = (uint16_t)(0x0600 + soloID),
// .type = SDO_WRITE_COMMAND,
// .code = CONTROL_MODE_CODE,
// .payload = MODE_SPEED
// };
// }

// data = (struct CanOpenData) {
// .id = (uint16_t)(0x0600 + soloID),
// .type = SDO_WRITE_COMMAND,
// .code = CONTROL_MODE_CODE,
// .payload = MODE_SPEED
// };

// can.write(createMessage(data));
// delayMicroseconds(200);

// set control mode
data = (struct CanOpenData) {
.id = (uint16_t)(0x0600 + soloID),
.type = SDO_WRITE_COMMAND,
.code = CONTROL_MODE_CODE,
.payload = MODE_SPEED
};
can.write(createMessage(data));
delayMicroseconds(200);

// set speed
data = (struct CanOpenData) {
.id = (uint16_t)(0x0600 + soloID),
.type = SDO_WRITE_COMMAND,
.code = SPEED_REF_CODE,
.payload = speedMag
};

can.write(createMessage(data));
delayMicroseconds(200);

// set direction
data = (struct CanOpenData) {
Expand All @@ -167,9 +150,7 @@ namespace solo_can {
.code = MOTOR_DIRECTION_CODE,
.payload = dir
};

can.write(createMessage(data));
delayMicroseconds(200);
}

void SetTorqueReferenceCommand(int soloID, int torqueRef, bool isReversed) {
Expand Down Expand Up @@ -223,5 +204,6 @@ namespace solo_can {

private:
FlexCAN_T4<_bus, _rxSize, _txSize> &can;
std::queue<CAN_message_t> sendQueue;
};
}
10 changes: 5 additions & 5 deletions platformio.ini
View file
Original file line number Diff line number Diff line change
@@ -1,18 +1,18 @@
[platformio]
default_envs = SoloComputerControl
default_envs = Main

[env:main]


[env:Main]
platform = teensy
board = teensy41
upload_protocol = teensy-cli
framework = arduino
check_tool = cppcheck
build_src_filter =
+<main/*>
+<Main/*>
lib_deps =
Nanopb@^0.4.7
QNEthernet@^0.26.0
https://github.com/basicmicro/roboclaw_arduino_library.git
custom_nanopb_protos =
+<protos/urc.proto>

Expand Down
69 changes: 69 additions & 0 deletions protos/urc.proto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,30 @@ message DriveEncodersMessage {
required int32 timestamp = 3;
}

message DrivetrainRequest {
required int32 m1Setpoint = 1;
required int32 m2Setpoint = 2;
required int32 m3Setpoint = 3;
required int32 m4Setpoint = 4;
required int32 m5Setpoint = 5;
required int32 m6Setpoint = 6;
}

message DrivetrainResponse {
required int32 m1Feedback = 1;
required uint32 m1Current = 2;
required int32 m2Feedback = 3;
required uint32 m2Current = 4;
required int32 m3Feedback = 5;
required uint32 m3Current = 6;
required int32 m4Feedback = 7;
required uint32 m4Current = 8;
required int32 m5Feedback = 9;
required uint32 m5Current = 10;
required int32 m6Feedback = 11;
required uint32 m6Current = 12;
}

message RequestMessage {
required bool requestSpeed = 1;
required bool requestDiagnostics = 2;
Expand All @@ -30,6 +54,51 @@ message StatusLightCommand {
optional int32 display = 2;
}

message NewStatusLightCommand {
required int32 redEnabled = 1;
required int32 blueEnabled = 2;
required int32 greenEnabled = 3;
required int32 redBlink = 4;
required int32 blueBlink = 5;
required int32 greenBlink = 6;
}

// message TeensyMessage {
// // 0 = drivetrain
// // 1 = status light
// required int32 messageID = 1;
// oneof payload {
// DrivetrainRequest driveEncodersMessage = 2;
// NewStatusLightCommand statusLightCommand = 3;
// }
// }

// message TeensyMessage {
// // 0 = drivetrain
// // 1 = status light
// required int32 messageID = 1;
// required DrivetrainRequest driveEncodersMessage = 2;
// required NewStatusLightCommand statusLightCommand = 3;
// }

message TeensyMessage {
// 0 = drivetrain
// 1 = status light
required int32 messageID = 1;
required int32 m1Setpoint = 2;
required int32 m2Setpoint = 3;
required int32 m3Setpoint = 4;
required int32 m4Setpoint = 5;
required int32 m5Setpoint = 6;
required int32 m6Setpoint = 7;
required int32 redEnabled = 8;
required int32 blueEnabled = 9;
required int32 greenEnabled = 10;
required int32 redBlink = 11;
required int32 blueBlink = 12;
required int32 greenBlink = 13;
}

message ArmClawRequest {
optional int32 clawVel = 1;
}
Expand Down
Loading

0 comments on commit 9f5547a

Please sign in to comment.