forked from johnkok/ros_odrive
-
Notifications
You must be signed in to change notification settings - Fork 0
/
odrive.cpp
178 lines (130 loc) · 5.76 KB
/
odrive.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
include "ros_odrive/odrive.hpp"
//using namespace std;
//check if need to declare std stuff if using namespace odrive
using namespace odrive; //added stuff to odrive.hpp with this namespace
Json::Value odrive_json; //define odrive_json type to json
bool targetJsonValid = false;
//endpoint is a pointer to an object of class odrive_endpoint
//odrive_endpoint *endpoint[2] = NULL; //Here or below?
ODriveDriver::ODriveDriver(const std::string* odrive_serial_numbers,
const uint8_t num_odrives,
const std::string* motor_to_odrive_serial_number_map, //array of 4 containing motor serial numbers
const uint8_t* motor_index_map, //array of 4 containing if motor is axis 0 or 1
const uint8_t num_motors) {
// read settings
num_odrives_ = num_odrives;
num_motors_ = num_motors;
// saved for use between creation and init
// save serial numbers to array of size 2
odrive_serial_numbers_ = new std::string[num_odrives]();
for (uint8_t i = 0; i < num_odrives; ++i) {
odrive_serial_numbers_[i].assign(odrive_serial_numbers[i]);
}
motor_to_odrive_serial_number_map_ = new std::string[num_motors]();
motor_index_map_ = new uint8_t[num_motors]();
for (uint8_t i = 0; i < num_motors; ++i) {
//save array containing serial numbers of each motor //////USE THIS OR MOTOR TO ODRIVE MAP??
motor_to_odrive_serial_number_map_[i].assign(motor_to_odrive_serial_number_map[i]);
//save array saying motor axis
motor_index_map_[i] = motor_index_map[i];
}
//Define if its odrive 0 or 1 -
uint8_t motor_to_odrive_map[4] = {0, 0, 1, 1};
}
//This is a destructor - used to shut stuff down
ODriveDriver::~ODriveDriver() {
for (uint8_t i = 0; i < 2; i++){
endpoint[i]->remove();
delete endpoint[i];
}
if (motor_index_map_) delete [] motor_index_map_;
if (odrive_serial_numbers_) delete [] odrive_serial_numbers_;
if (motor_to_odrive_serial_number_map_) delete [] motor_to_odrive_serial_number_map_;
return 0;
}
/**
*
* Initialise odrives
*
*/
int ODriveDriver::init() {
//The part below of endpoints and json is what I need to modify for 2 boards
// Get odrive endpoint instance
//endpoint needs to be an array of 2 containing the endpoints for each serial number
endpoint0 = new odrive_endpoint();
endpoint1 = new odrive_endpoint();
odrive_endpoint *endpoint[2] = {endpoint0, endpoint1};
for (i = 0; i < 2; ++i) {
// Enumerate Odrive target
if (endpoint[i]->init(stoull(odrive_serial_numbers_[i], 0, 16)))
{
ROS_ERROR("Device not found!");
return 1;
}
// Read JSON from target - How to save odrive_json 1 and 2 - reading and saving separate odrive_json for each target will be a better solution.
//getJson(endpoint, address of jsonvalue odrive_json)
if (getJson(endpoint[i], &odrive_json)) {
return 1;
}
targetJsonValid = true;
}
//Got rid of watchdog as diff drive supposedly has a timeout thing
}
int ODriveDriver::setMotorSpeed(int motor_index, float motor_speed) {
std::string cmd;
//Get which axis depending on index
cmd = "axis"
cmd.append(std::to_string(motor_index_map_[motor_index]))
//odrive json will depend on odrive being targeted
writeOdriveData(endpoint[motor_to_odrive_map[motor_index]], odrive_json, cmd.append(".controller.vel_setpoint"), motor_speed);
}
int ODriveDriver::setMotorSpeeds(float* motor_speeds) {
for (uint8_t i = 0; i < num_motors_; ++i) {
setMotorSpeed(i, motor_speeds[i]);
}
}
//Get encoder position reading for motor
int ODriveDriver::getPosCPR(int motor_index, float &pos) {
std::string cmdpos;
cmdpos = "axis"
cmdpos.append(std::to_string(motor_index_map_[motor_index]))
//Encoder Position
readOdriveData(endpoint[motor_to_odrive_map[motor_index]], odrive_json, cmdpos.append(".encoder.pos_estimate"), pos);
}
//Get encoder speed reading for motor
int ODriveDriver::getMotorSpeed(int motor_index, float &motor_speed) {
std::string cmdvel;
cmdvel = "axis"
cmdvel.append(std::to_string(motor_index_map_[motor_index]))
//Encoder Velocity
readOdriveData(endpoint[motor_to_odrive_map[motor_index]], odrive_json, cmdvel.append("encoder.vel_estimate"), motor_speed);
}
/***Not used for now
int ODriveDriver::getBusVoltage(int motor_index, float &voltage) {
getFloat(motor_index, voltage, VBUS_VOLTAGE);
std::cout << "Voltage: " << motor_index << " : " << voltage << std::endl;
//To this
readOdriveData(endpoint, odrive_json, string("vbus_voltage"), fval);
msg.vbus = fval;
}
int ODriveDriver::checkErrors(uint8_t* error_codes_array) {
int cmd;
int axis_offset;
for (uint8_t i = 0; i < num_motors_; ++i) {
axis_offset = (motor_index_map_[i] == 1) ? per_axis_offset : 0;
cmd = AXIS__ERROR + axis_offset;
uint8_t handle_index = motor_to_odrive_handle_index_[i];
uint8_t motor_error_output;
readOdriveData(endpoint, odrive_json, string("axis0.error"), u16val);
msg.error0 = u16val;
readOdriveData(endpoint, odrive_json, string("axis1.error"), u16val);
msg.error1 = u16val;
int result = odriveEndpointGetUInt8(odrive_handles_[handle_index], cmd, motor_error_output);
if (result != LIBUSB_SUCCESS) {
std::cerr << "CppSdk::checkErrors couldn't send `" << std::to_string(cmd) << "` to '" << odrive_serial_numbers_[handle_index] << "': `" << result << "` (see prior error message)" << std::endl;
return ODRIVE_SDK_UNEXPECTED_RESPONSE;
}
error_codes_array[i] = motor_error_output;
}
return ODRIVE_SDK_COMM_SUCCESS;
}***/