Skip to content

Commit f05649c

Browse files
imisaacwuimisaacwu
committed
Callback fixes, encoder testing
1 parent 5b085cd commit f05649c

6 files changed

Lines changed: 56 additions & 22 deletions

File tree

‎src/CAN/CAN.cpp‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -209,8 +209,7 @@ void handleEncoderEstimates(CANPacket_t& packet) {
209209
CANDeviceUUID_t uuid = packet.senderUUID;
210210
// Convert position from revolutions to millidegrees
211211
int32_t positionMdeg = static_cast<int32_t>(decoded.position * Constants::MILLIDEGREES_PER_REV);
212-
213-
// LOG_F(INFO, "0x%x: %f mdeg", uuid, decoded.position);
212+
214213
telemetrycode_t telemCode = static_cast<telemetrycode_t>(telemtype_t::angle);
215214
storeTelemetry(uuid, telemCode, robot::types::DataPoint<telemetry_t>(positionMdeg));
216215
}
@@ -581,6 +580,10 @@ void addDirectReadCallback(CANDevice_t device, uint16_t endpoint, const std::fun
581580
auto key = std::make_pair(static_cast<uint8_t>(device.deviceUUID), endpoint);
582581
// Write access
583582
std::unique_lock mapLock(directReadMapMutex);
583+
if (auto it = directReadCallbackMap.find(key); it != directReadCallbackMap.end()) {
584+
LOG_F(WARNING, "Callback already exists for 0x%x endpoint %d! Ignoring..", device.deviceUUID, endpoint);
585+
return;
586+
}
584587
directReadCallbackMap.insert({key, callback});
585588
}
586589

‎src/CAN/CAN.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -43,7 +43,7 @@ enum class input_mode_t : uint8_t {
4343
};
4444

4545
/** @brief ODrive Axis States */
46-
enum class axis_state_t : uint32_t {
46+
enum class axis_state_t : uint8_t {
4747
idle = BLDC_AXIS_IDLE,
4848
full_calib = BLDC_AXIS_FULL_CALIBRATION_SEQUENCE,
4949
motor_calib = BLDC_AXIS_MOTOR_CALIBRATION,

‎src/CAN/CANBoard.cpp‎

Lines changed: 29 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@ CANBoard::CANBoard(robot::types::boardid_t board_id, CANDevice_t device)
2222
uint16_t endpoint_id = endpoint["id"];
2323
addDirectReadCallback(this->device, endpoint_id, [this, endpoint_id](auto p) {
2424
this->vel_limit = p.value_float;
25+
LOG_F(INFO, "Fetched vel limit for 0x%x: %f", this->device.deviceUUID, this->vel_limit);
2526

2627
// We only need this once, remove after we get a response
2728
removeDirectReadCallback(this->device, endpoint_id);
@@ -47,12 +48,12 @@ CANBoard::CANBoard(robot::types::boardid_t board_id, CANDevice_t device)
4748
this->inversion_factor = it->second;
4849
}
4950

50-
if (nlohmann::json endpoint = getEndpoint(this->board_id, "axis0.pos_estimate"); endpoint != nullptr) {
51-
uint16_t endpoint_id = endpoint["id"];
52-
addDirectReadCallback(this->device, endpoint_id, [this, endpoint_id](auto p) {
53-
LOG_F(INFO, "0x%x at %f rots", this->device.deviceUUID, p.value_float);
54-
});
55-
}
51+
// if (nlohmann::json endpoint = getEndpoint(this->board_id, "axis0.pos_estimate"); endpoint != nullptr) {
52+
// uint16_t endpoint_id = endpoint["id"];
53+
// addDirectReadCallback(this->device, endpoint_id, [this, endpoint_id](auto p) {
54+
// LOG_F(INFO, "0x%x at %f rots", this->device.deviceUUID, p.value_float);
55+
// });
56+
// }
5657
}
5758

5859
if (device.peripheralDomain) {
@@ -75,6 +76,25 @@ void CANBoard::setMotorPower(double power) {
7576
if (power == 0.0) {
7677
if (static_cast<uint8_t>(this->board_id) < 5) { // hack for wheels + base
7778
this->setMotorState(can::motor::axis_state_t::idle);
79+
80+
// hang until this actually goes idle for funsies
81+
this->correct = false;
82+
if (nlohmann::json endpoint = getEndpoint(this->board_id, "axis0.current_state"); endpoint != nullptr) {
83+
uint16_t endpoint_id = endpoint["id"];
84+
addDirectReadCallback(this->device, endpoint_id, [this, endpoint_id](auto decoded) {
85+
if (decoded.value_uint8 != static_cast<uint8_t>(can::motor::axis_state_t::idle)) {
86+
LOG_F(ERROR, "0x%x DID NOT LISTEN AND IS NOT IDLE", this->device.deviceUUID);
87+
this->setMotorState(can::motor::axis_state_t::idle);
88+
} else {
89+
removeDirectReadCallback(this->device, endpoint_id);
90+
correct = true;
91+
}
92+
});
93+
94+
this->read(endpoint_id);
95+
}
96+
97+
while (!this->correct);
7898
} else if (this->board_id == robot::types::boardid_t::shoulder || this->board_id == robot::types::boardid_t::elbow) {
7999
// Set motor general lockin vel to 0
80100
this->setMotorState(can::motor::axis_state_t::lockin_spin);
@@ -93,12 +113,12 @@ void CANBoard::setMotorPower(double power) {
93113

94114
if (nlohmann::json endpoint = getEndpoint(this->board_id, "axis0.controller.input_vel"); endpoint != nullptr) {
95115
uint16_t endpoint_id = endpoint["id"];
96-
addDirectReadCallback(this->device, endpoint_id, [input_vel](auto decoded) {
116+
addDirectReadCallback(this->device, endpoint_id, [input_vel, p, this, endpoint_id](auto decoded) {
97117
if (decoded.value_float != input_vel) {
98118
LOG_F(ERROR, "Expected %f, got %f", input_vel, decoded.value_float);
99-
// send
119+
sendCANPacket(p);
100120
} else {
101-
// remove from map
121+
removeDirectReadCallback(this->device, endpoint_id);
102122
}
103123
});
104124

‎src/CAN/CANBoard.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,8 +26,11 @@ class CANBoard {
2626
int8_t inversion_factor;
2727

2828
// Configs read on startup
29-
uint8_t vel_limit;
29+
float vel_limit;
3030
bool watchdog;
31+
32+
// debug
33+
bool correct = false;
3134
};
3235

3336
} // namespace can

‎src/CAN/FakeCANBoard.cpp‎

Lines changed: 15 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -52,22 +52,28 @@ int main() {
5252
uint16_t uuid = static_cast<uint16_t>(prompt("Enter device uuid"));
5353
// TODO: Assuming motor domain for now
5454
CANDevice_t device = CANDevice_t{0, 1, 0, uuid};
55-
// Change to debug2 for pro endpoints
56-
std::shared_ptr<can::CANBoard> board = std::make_shared<can::CANBoard>(robot::types::boardid_t::debug1, device);
55+
56+
uint8_t type = static_cast<uint8_t>(prompt("0 for S1, 1 for Pro"));
57+
std::shared_ptr<can::CANBoard> board = std::make_shared<can::CANBoard>(
58+
type == 0 ? robot::types::boardid_t::debug1 : robot::types::boardid_t::debug2, device);
5759

5860
while (true) {
5961
if (testMode == TestMode::State) {
6062
std::stringstream state_msg("Enter desired motor state:\n");
61-
state_msg << static_cast<uint32_t>(can::motor::axis_state_t::idle) << " idle\n";
62-
state_msg << static_cast<uint32_t>(can::motor::axis_state_t::closed_loop_control) << " closed loop control\n";
63+
state_msg << static_cast<uint8_t>(can::motor::axis_state_t::idle) << " idle\n";
64+
state_msg << static_cast<uint8_t>(can::motor::axis_state_t::closed_loop_control) << " closed loop control\n";
6365

6466
int state = prompt(state_msg.str().c_str());
6567
can::motor::axis_state_t motor_state = static_cast<can::motor::axis_state_t>(state);
6668
board->setMotorState(motor_state);
6769
} else if (testMode == TestMode::Power) {
68-
board->setMotorState(can::motor::axis_state_t::closed_loop_control);
70+
// board->setMotorState(can::motor::axis_state_t::closed_loop_control);
6971

70-
double power = static_cast<double>(prompt("Enter power"));
72+
// double power = static_cast<double>(prompt("Enter power"));
73+
std::string input;
74+
std::cout << "Enter power [-1.0, 1.0]: ";
75+
std::getline(std::cin, input);
76+
float power = std::stof(input);
7177
board->setMotorPower(power);
7278
} else if (testMode == TestMode::Read) {
7379
// uint16_t endpoint = static_cast<uint16_t>(prompt("Enter endpoint name"));
@@ -80,9 +86,10 @@ int main() {
8086
// std::cout << "endpoint " << input << " has id=" << endpoint_id << std::endl;
8187
can::addDirectReadCallback(board->get_device(), endpoint_id, [board, input, endpoint](auto decoded) {
8288
std::stringstream rs("");
83-
rs << input << " from 0x" << std::hex << board->get_device().deviceUUID << ": ";
89+
rs << input << " from 0x" << std::hex << board->get_device().deviceUUID << " [";
8490

8591
std::string type = endpoint["type"];
92+
rs << type << "]: ";
8693
if (type == "uint32") {
8794
rs << decoded.value_uint32;
8895
} else if (type == "int32") {
@@ -97,6 +104,7 @@ int main() {
97104
rs << (decoded.value_bool ? "true" : "false");
98105
}
99106

107+
100108
std::cout << rs.str().c_str() << std::endl;
101109

102110
can::removeDirectReadCallback(board->get_device(), endpoint["id"]);

‎src/world_interface/real_world_constants.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -125,8 +125,8 @@ constexpr auto boardInversionMap = frozen::make_unordered_map<boardid_t, uint8_t
125125
// Hand (0x60)
126126
{boardid_t::hand, 1},
127127
// DEBUG (0x70, 0x71)
128-
{boardid_t::debug1, 0},
129-
{boardid_t::debug2, 0}
128+
{boardid_t::debug1, 1},
129+
{boardid_t::debug2, 1}
130130
});
131131

132132
constexpr auto proBoards = frozen::make_unordered_set<boardid_t>({

0 commit comments

Comments
 (0)