Skip to content

Commit 68c4085

Browse files
imisaacwuimisaacwu
committed
Add braking
1 parent 09819b1 commit 68c4085

8 files changed

Lines changed: 114 additions & 55 deletions

File tree

‎src/CAN/CAN.cpp‎

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -116,12 +116,11 @@ bool receivePacket(int fd, CANPacket_t& packet) {
116116
}
117117

118118
void handleAck(CANPacket_t& packet) {
119-
printCANPacket(packet);
120119
auto decoded = CANUniversalPacket_Acknowledge_Decode(&packet);
121120
if (decoded.failure) {
122-
LOG_F(WARNING, "Ack received from 0x%x: FAIL", decoded.receiver.deviceUUID);
121+
LOG_F(WARNING, "Ack received from 0x%x: FAIL", decoded.sender.deviceUUID);
123122
} else {
124-
LOG_F(INFO, "Ack received from 0x%x: ok", decoded.receiver.deviceUUID);
123+
// LOG_F(INFO, "Ack received from 0x%x: ok", decoded.sender.deviceUUID);
125124
}
126125

127126
auto key = std::make_pair(static_cast<uint8_t>(decoded.sender.deviceUUID), decoded.commandID);
@@ -135,7 +134,6 @@ void handleAck(CANPacket_t& packet) {
135134

136135
void handleDirectRead(CANPacket_t& packet) {
137136
auto decoded = CANMotorPacket_BLDC_DirectReadResult_Decode(&packet);
138-
// LOG_F(INFO, "Read for 0x%x of %x", decoded.sender.deviceUUID, decoded.endpointID);
139137

140138
// Fire off callback, if it exists
141139
auto key = std::make_pair(static_cast<uint8_t>(decoded.sender.deviceUUID), decoded.endpointID);
@@ -292,7 +290,6 @@ void processThreadFn() {
292290

293291
// Double check required after releasing lock
294292
if (buffer.empty()) { continue; }
295-
// LOG_F(INFO, "Buffer size = %ld", buffer.size());
296293
CANPacket_t packet = buffer.front();
297294
buffer.pop();
298295
// Done with buffer, unlock to allow more reading
@@ -360,6 +357,7 @@ void initCAN() {
360357

361358
void sendCANPacket(const CANPacket_t& packet) {
362359
CANPacket_t mutablePacket = packet; // to pass, we make a mutable copy
360+
mutablePacket.command = CAN_ACK(packet.command);
363361
CANDeviceUUID_t uuid = packet.device.deviceUUID;
364362
canfd_frame frame;
365363
std::memset(&frame, 0, sizeof(frame));

‎src/CAN/CANBoard.cpp‎

Lines changed: 34 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ CANBoard::CANBoard(robot::types::boardid_t board_id, CANDevice_t device)
6161

6262
void CANBoard::setMotorPower(double power) {
6363
if (!this->device.motorDomain) {
64-
LOG_F(WARNING, "setMotorPower called for board 0x%x not in motor domain!", this->device.deviceUUID);
64+
LOG_F(WARNING, "setMotorPower called for %s board not in motor domain!", util::to_string(this->board_id).c_str());
6565
return;
6666
}
6767

@@ -72,14 +72,19 @@ void CANBoard::setMotorPower(double power) {
7272
this->setMotorState(can::motor::axis_state_t::closed_loop_control);
7373

7474
if (power == 0.0) {
75+
if (this->board_id == robot::types::boardid_t::shoulder || this->board_id == robot::types::boardid_t::elbow) {
76+
// Set brake
77+
this->setBrake(BRAKE_ON);
78+
}
79+
7580
if (this->watchdog) {
7681
this->setMotorState(can::motor::axis_state_t::idle);
7782

7883
if (nlohmann::json endpoint = getEndpoint(this->board_id, "axis0.current_state"); endpoint != nullptr) {
7984
uint16_t endpoint_id = endpoint["id"];
8085
addDirectReadCallback(this->device, endpoint_id, [this, endpoint_id](auto decoded) {
8186
if (decoded.value_uint8 != static_cast<uint8_t>(can::motor::axis_state_t::idle)) {
82-
LOG_F(ERROR, "0x%x DID NOT LISTEN AND IS NOT IDLE AND IS INSTEAD %d", this->device.deviceUUID, decoded.value_uint8);
87+
LOG_F(ERROR, "0x%x (%s) DID NOT LISTEN AND IS NOT IDLE AND IS INSTEAD %d, re-attempting...", this->device.deviceUUID, util::to_string(this->board_id).c_str(), decoded.value_uint8);
8388
this->setMotorState(can::motor::axis_state_t::idle);
8489
this->read(endpoint_id);
8590
} else {
@@ -89,19 +94,18 @@ void CANBoard::setMotorPower(double power) {
8994

9095
this->read(endpoint_id);
9196
}
92-
} else if (this->board_id == robot::types::boardid_t::shoulder || this->board_id == robot::types::boardid_t::elbow) {
93-
// Set motor general lockin vel to 0
94-
this->setMotorState(can::motor::axis_state_t::lockin_spin);
95-
}
96-
97-
if (nlohmann::json endpoint = getEndpoint(this->board_id, "axis0.controller.input_vel"); endpoint != nullptr) {
98-
uint16_t endpoint_id = endpoint["id"];
99-
removeDirectReadCallback(this->device, endpoint_id);
10097
}
10198
} else {
10299
// Mapping power to a target velocity
103-
float input_vel = static_cast<float>(power * this->vel_limit);
100+
float input_vel = static_cast<float>(power * this->vel_limit) * 0.4; // hard-coded 40%
104101
input_vel *= this->inversion_factor;
102+
103+
// LOG_F(INFO, "True input velocity %f, ", input_vel);
104+
105+
if (this->board_id == robot::types::boardid_t::shoulder || this->board_id == robot::types::boardid_t::elbow) {
106+
this->setBrake(BRAKE_OFF);
107+
}
108+
105109
// Make CANPacket_t
106110
CANPacket_t p = CANMotorPacket_BLDC_SetInputVelocity(
107111
Constants::JETSON_DEVICE, this->device, input_vel, 0.0f
@@ -116,6 +120,8 @@ void CANBoard::setMotorPower(double power) {
116120
if (decoded.value_float != input_vel) {
117121
LOG_F(ERROR, "Expected %f, got %f", input_vel, decoded.value_float);
118122
sendCANPacket(p);
123+
} else {
124+
LOG_F(INFO, "Got %f vel_limit :)", decoded.value_float);
119125
}
120126
});
121127

@@ -126,7 +132,7 @@ void CANBoard::setMotorPower(double power) {
126132

127133
void CANBoard::setMotorState(can::motor::axis_state_t state) {
128134
if (!this->device.motorDomain) {
129-
LOG_F(WARNING, "setMotorState called for board 0x%x not in motor domain!", this->device.deviceUUID);
135+
LOG_F(WARNING, "setMotorState called for %s board not in motor domain!", util::to_string(this->board_id).c_str());
130136
return;
131137
}
132138

@@ -139,7 +145,7 @@ void CANBoard::setMotorState(can::motor::axis_state_t state) {
139145

140146
void CANBoard::setMotorVel(int8_t velocity) {
141147
if (!this->device.motorDomain) {
142-
LOG_F(WARNING, "setMotorVel called for board 0x%x not in motor domain!", this->device.deviceUUID);
148+
LOG_F(WARNING, "setMotorVel called for %s board not in motor domain!", util::to_string(this->board_id).c_str());
143149
return;
144150
}
145151

@@ -156,7 +162,7 @@ void CANBoard::setMotorVel(int8_t velocity) {
156162

157163
void CANBoard::setStepperRevs(float revs) {
158164
if (!this->device.motorDomain) {
159-
LOG_F(WARNING, "setStepperRevs called for board 0x%x not in motor domain!", this->device.deviceUUID);
165+
LOG_F(WARNING, "setStepperRevs called for %s board not in motor domain!", util::to_string(this->board_id).c_str());
160166
return;
161167
}
162168

@@ -166,6 +172,20 @@ void CANBoard::setStepperRevs(float revs) {
166172
sendCANPacket(p);
167173
}
168174

175+
void CANBoard::setBrake(uint8_t state) {
176+
auto it = robot::boardBrakeIDMap.find(this->board_id);
177+
if (it == robot::boardBrakeIDMap.end()) {
178+
LOG_F(WARNING, "setBrake called for %s that does not have a brake!", util::to_string(this->board_id).c_str());
179+
return;
180+
}
181+
182+
// hack, but we only have one braking board sooo
183+
CANPacket_t p = CANPeripheralPacket_SetBrakes(
184+
Constants::JETSON_DEVICE, CANDevice_t{1, 0, 0, CAN_UUID_TELEMETRY}, it->second, state
185+
);
186+
sendCANPacket(p);
187+
}
188+
169189
void CANBoard::read(uint16_t endpoint) {
170190
CANPacket_t p = CANMotorPacket_BLDC_DirectRead(
171191
Constants::JETSON_DEVICE, this->device, endpoint

‎src/CAN/CANBoard.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,9 @@
66
#include <mutex>
77
#include <shared_mutex>
88

9+
#define BRAKE_ON 0
10+
#define BRAKE_OFF 1
11+
912
namespace can {
1013

1114
class CANBoard {
@@ -18,6 +21,9 @@ class CANBoard {
1821
void setMotorVel(int8_t velocity);
1922
void setStepperRevs(float revs);
2023

24+
// Peripheral
25+
void setBrake(uint8_t state);
26+
2127
// Universal
2228
void read(uint16_t endpoint);
2329

‎src/CAN/FakeCANBoard.cpp‎

Lines changed: 37 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,9 @@ enum class TestMode {
99
Power,
1010
Read,
1111
Stepper,
12+
Peripheral,
1213
RawCAN,
14+
Debug,
1315
NUM_MODES
1416
};
1517

@@ -42,6 +44,7 @@ int main() {
4244
ss << static_cast<int>(TestMode::Power) << " for POWER CONTROL\n";
4345
ss << static_cast<int>(TestMode::Read) << " for DIRECT READ\n";
4446
ss << static_cast<int>(TestMode::Stepper) << " for STEPPER\n";
47+
ss << static_cast<int>(TestMode::Peripheral) << " for PERIPHERAL\n";
4548
ss << static_cast<int>(TestMode::RawCAN) << " for RAW CAN\n";
4649

4750
while (true) {
@@ -146,43 +149,49 @@ int main() {
146149
CANPacket_t packet = CANMotorPacket_Stepper_DriveRevolutions(Constants::JETSON_DEVICE, device, revs);
147150
can::printCANPacket(packet);
148151
can::sendCANPacket(packet);
149-
} else if (testMode == TestMode::RawCAN) {
152+
} else if (testMode == TestMode::Peripheral) {
150153
uint8_t periphID = prompt("peripheral ID");
151154
std::string input;
152155
std::cout << "Enter pwm duty cycle: ";
153156
std::getline(std::cin, input);
154157
float dutyCycle = std::stof(input);
155158
CANPacket_t packet = CANPeripheralPacket_SetPWMDutyCycle(Constants::JETSON_DEVICE, device, periphID, dutyCycle);
156159
packet.command = CAN_ACK(packet.command);
160+
} else if (testMode == TestMode::RawCAN) {
161+
uint8_t pr = prompt("priority");
162+
uint8_t command = prompt("command");
163+
uint8_t dlc = prompt("add'l. data bits");
164+
if (dlc > 5) {
165+
std::cout << "Too many data bits" << std::endl;
166+
continue;
167+
}
168+
uint8_t data[dlc + 1];
169+
data[0] = command;
170+
171+
for (int i = 1; i <= dlc; i++) {
172+
data[i] = prompt("bit " + std::to_string(i));
173+
}
174+
175+
// // manual construction of a generic packet
176+
CANPacket_t p = {};
177+
p.device = device;
178+
p.priority = static_cast<CANPriority_t>(pr);
179+
p.command = command;
180+
p.senderUUID = CAN_UUID_JETSON;
181+
p.contentsLength = dlc;
182+
for (int i = 0; i < p.contentsLength && i < 6; i++) {
183+
p.contents[i] = data[i + 1];
184+
}
185+
186+
can::printCANPacket(p);
187+
can::sendCANPacket(p);
188+
} else if (testMode == TestMode::Debug) {
189+
uint8_t brakeID = prompt("brake ID");
190+
uint8_t state = prompt("state");
191+
CANPacket_t packet = CANPeripheralPacket_SetBrakes(Constants::JETSON_DEVICE, device, brakeID, state);
192+
157193
can::printCANPacket(packet);
158194
can::sendCANPacket(packet);
159-
// uint8_t pr = prompt("priority");
160-
// uint8_t command = prompt("command");
161-
// uint8_t dlc = prompt("add'l. data bits");
162-
// if (dlc > 5) {
163-
// std::cout << "Too many data bits" << std::endl;
164-
// continue;
165-
// }
166-
// uint8_t data[dlc + 1];
167-
// data[0] = command;
168-
169-
// for (int i = 1; i <= dlc; i++) {
170-
// data[i] = prompt("bit " + std::to_string(i));
171-
// }
172-
173-
// // // manual construction of a generic packet
174-
// CANPacket_t p = {};
175-
// p.device = device;
176-
// p.priority = static_cast<CANPriority_t>(pr);
177-
// p.command = command;
178-
// p.senderUUID = CAN_UUID_JETSON;
179-
// p.contentsLength = dlc;
180-
// for (int i = 0; i < p.contentsLength && i < 6; i++) {
181-
// p.contents[i] = data[i + 1];
182-
// }
183-
184-
// can::printCANPacket(p);
185-
// can::sendCANPacket(p);
186195
}
187196
}
188197
}

‎src/control_interface.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -191,7 +191,6 @@ void setJointMotorPower(robot::types::jointid_t joint, double power) {
191191
setMotorPower(boardid_t::wristDiffLeft, gearPwr.left);
192192
setMotorPower(boardid_t::wristDiffRight, gearPwr.right);
193193
} else if (joint == jointid_t::hand) {
194-
LOG_F(INFO, "Hand stepper");
195194
setStepperRevs(boardid_t::hand, static_cast<float>(power));
196195
} else {
197196
LOG_F(WARNING, "setJointPower called for currently unsupported joint %s",

‎src/world_interface/data.cpp‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,27 @@ std::bitset<N_LIMIT_SWITCH> LimitSwitchData::diff(const LimitSwitchData& other)
2727

2828
namespace util {
2929

30+
std::string to_string(robot::types::boardid_t board) {
31+
using robot::types::boardid_t;
32+
switch (board) {
33+
case boardid_t::frontTireLeft: return "frontTireLeft";
34+
case boardid_t::frontTireRight: return "frontTireRight";
35+
case boardid_t::rearTireLeft: return "rearTireLeft";
36+
case boardid_t::rearTireRight: return "rearTireRight";
37+
case boardid_t::armBase: return "armBase";
38+
case boardid_t::shoulder: return "shoulder";
39+
case boardid_t::elbow: return "elbow";
40+
case boardid_t::forearm: return "forearm";
41+
case boardid_t::wristDiffLeft: return "wristDiffLeft";
42+
case boardid_t::wristDiffRight: return "wristDiffRight";
43+
case boardid_t::telemetry: return "telemetry";
44+
case boardid_t::hand: return "hand";
45+
case boardid_t::debug1: return "debug1";
46+
case boardid_t::debug2: return "debug2";
47+
default: return "<unknown>";
48+
}
49+
}
50+
3051
std::string to_string(robot::types::jointid_t joint) {
3152
using robot::types::jointid_t;
3253
switch (joint) {

‎src/world_interface/data.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -333,6 +333,7 @@ class LimitSwitchData {
333333
} // namespace robot::types
334334

335335
namespace util {
336+
std::string to_string(robot::types::boardid_t board);
336337
std::string to_string(robot::types::jointid_t joint);
337338
std::string to_string(const robot::types::CameraID& id);
338339
std::string to_string(robot::types::mountedperipheral_t peripheral);

‎src/world_interface/real_world_constants.h‎

Lines changed: 12 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,6 @@
55
#include "../Constants.h"
66
#include "data.h"
77

8-
#include <CANDevices.h>
98
#include <chrono>
109
#include <cstdint>
1110
#include <unordered_map>
@@ -120,7 +119,7 @@ constexpr auto UUIDBoardMap = frozen::make_unordered_map<CANDeviceUUID_t, boardi
120119
{CAN_UUID_BLDC_FOREARM, boardid_t::forearm},
121120
});
122121

123-
constexpr auto boardInversionMap = frozen::make_unordered_map<boardid_t, uint8_t>({
122+
constexpr auto boardInversionMap = frozen::make_unordered_map<boardid_t, int8_t>({
124123
{boardid_t::frontTireLeft, -1},
125124
{boardid_t::frontTireRight, 1},
126125
{boardid_t::rearTireLeft, -1},
@@ -148,6 +147,12 @@ constexpr auto proBoards = frozen::make_unordered_set<boardid_t>({
148147
boardid_t::debug2,
149148
});
150149

150+
constexpr auto boardBrakeIDMap = frozen::make_unordered_map<boardid_t, uint8_t>({
151+
{boardid_t::armBase, 5},
152+
{boardid_t::shoulder, 2},
153+
{boardid_t::elbow, 1}
154+
});
155+
151156
/** @brief A mapping of PID controlled motors to their pid coefficients. */
152157
constexpr auto motorPIDMap =
153158
frozen::make_unordered_map<boardid_t, pidcoef_t>({{boardid_t::shoulder, {70, 0, 0}}});
@@ -157,10 +162,10 @@ constexpr auto motorPIDMap =
157162
* Negative values mean that the motor is inverted.
158163
*/
159164
constexpr auto positive_pwm_scales =
160-
frozen::make_unordered_map<boardid_t, double>({{boardid_t::armBase, 10},
165+
frozen::make_unordered_map<boardid_t, double>({{boardid_t::armBase, 1},
161166
{boardid_t::shoulder, -1},
162167
{boardid_t::elbow, -1},
163-
{boardid_t::forearm, -0.1},
168+
{boardid_t::forearm, -0.01},
164169
{boardid_t::wristDiffLeft, -0.1},
165170
{boardid_t::wristDiffRight, 0.1},
166171
{boardid_t::frontTireLeft, -3},
@@ -173,13 +178,13 @@ constexpr auto positive_pwm_scales =
173178
* Negative values mean that the motor is inverted.
174179
*/
175180
constexpr auto negative_pwm_scales =
176-
frozen::make_unordered_map<boardid_t, double>({{boardid_t::armBase, 10},
181+
frozen::make_unordered_map<boardid_t, double>({{boardid_t::armBase, -1},
177182
{boardid_t::shoulder, -1},
178183
{boardid_t::elbow, -1},
179-
{boardid_t::forearm, -0.1},
184+
{boardid_t::forearm, -0.01},
180185
{boardid_t::wristDiffLeft, -0.1},
181186
{boardid_t::wristDiffRight, 0.1},
182-
{boardid_t::frontTireLeft, -3},
187+
{ boardid_t::frontTireLeft, -3},
183188
{boardid_t::frontTireRight, 3},
184189
{boardid_t::rearTireLeft, -3},
185190
{boardid_t::rearTireRight, 3},

0 commit comments

Comments
 (0)