Permalink
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
387 lines (339 sloc) 12.5 KB
#include "Arduino.h"
#include "ODriveArduino.h"
// Print with stream operator
template<class T> inline Print& operator <<(Print &obj, T arg) { obj.print(arg); return obj; }
template<> inline Print& operator <<(Print &obj, float arg) { obj.print(arg, 4); return obj; }
/**
* Construct ODriveArduino object linked to the given serial port.
* @param serial Serial port to use to communicate to the ODrive
*/
ODriveArduino::ODriveArduino(HardwareSerial& serial)
: serial_(serial) {}
/**
* Set an ODrive property from the Teensy
* @param property The ODrive property to set
* @param value The value for the property
*
* Example: odrv0.SetProperty("axis0.motor.config.current_lim", "5.0f");
*/
void ODriveArduino::SetProperty(char* property, char* value) {
SendStartByte(); SendNLLen();
serial_ << "w " << property << " " << value << '\n';
}
/**
* Read an ODrive property from the Teensy
* @param property Property to query
* NOTE: You must somehow handle the reponse from the ODrive separately
*/
void ODriveArduino::ReadProperty(char* property) {
SendStartByte(); SendNLLen();
serial_ << "r " << property << "\n";
}
/**
* Set the current limits for both motors.
* @param current_lim Current limit
*/
void ODriveArduino::SetCurrentLims(float current_lim) {
SendStartByte(); SendNLLen();
serial_ << "w axis0.motor.config.current_lim " << current_lim << "\n";
SendStartByte(); SendNLLen();
serial_ << "w axis1.motor.config.current_lim " << current_lim << "\n";
}
void ODriveArduino::ReadCurrents() {
SendStartByte(); SendNLLen();
serial_ << "r axis0.motor.current_control.Iq_measured\n";
SendStartByte(); SendNLLen();
serial_ << "r axis1.motor.current_control.Iq_measured\n";
}
/**
* Send a message to the odrive that tells it to send back the vbus voltage
* Working as of 7/7/18
*/
void ODriveArduino::QueryVBusVoltage() {
SendStartByte(); SendNLLen();
serial_ << "r vbus_voltage\n";
}
/**
* Parses the encoder position message and stores positions as counts
* Assumes the message is in format "<1><6><'P'><short1><short2><checksum>"
* TODO: add pll_vel to the message for better motor velocity measurement!!!
* This would greatly improve the noise on the Kd term!!
* @param msg String: Message to parse
* @param m0 float&: Output parameter for axis0 reading
* @param m1 float&: Output parameter for axis1 reading
* @return int: 1 if success, -1 if failed to find get full message or checksum failed
*/
int ODriveArduino::ParseDualPosition(char* msg, int len, float& m0, float& m1) {
// check if 1 byte for "P", 4 bytes holding encoder data, and 1 checksum byte were received
if (len != 6) {
return -1; // return -1 to indicate that the message length was wrong
} else {
// retrieve short from byte stream
// remember that the first character is 'P'
uint16_t m0_16 = (msg[2] << 8) | msg[1];
uint16_t m1_16 = (msg[4] << 8) | msg[3];
uint8_t rcvdCheckSum = msg[5];
// compute checksum
uint8_t checkSum = 0;
checkSum ^= msg[0]; // letter 'P'
checkSum ^= msg[1];
checkSum ^= msg[2];
checkSum ^= msg[3];
checkSum ^= msg[4];
// DEBUG ONLY
// Serial << "Comp checksum: " << (int)checkSum << " rcvd: " << (int)rcvdCheckSum << "\n";
// if the computed and received check sums match then update the motor position variables
if (checkSum == rcvdCheckSum) {
// convert to float
// TODO: figure out if casts are needed
m0 = (float) ((int16_t) m0_16);
m1 = (float) ((int16_t) m1_16);
return 1;
} else {
// return -1 to indicate that the checksums didn't match
return -1;
}
}
return 1;
}
/**
* Send the start byte to indicate to the receiver that a new message is being sent
*/
void ODriveArduino::SendStartByte() {
serial_ << START_BYTE;
}
/**
* Send the payload length value (0) that indicates to the receiver to read until a newline is hit
*/
void ODriveArduino::SendNLLen() {
serial_ << NL_LEN;
}
/**
* XOR operation on first 8 bits and last 8 bits of a short
* @param val The short to work on
* @return Byte resulting from xoring the first 8 and last 8 bits.
*/
uint8_t XorShort(int16_t val) {
// bad to duplicate code :(
uint8_t v0 = val & 0xFF;
uint8_t v1 = (val >> 8) & 0xFF;
return v0 ^ v1;
}
/**
* Send a single byte over serial
* @param byte Byte to send to the ODrive
*/
void ODriveArduino::SendByte(uint8_t byte) {
serial_ << (char) byte;
}
/**
* Send a short (16 bit signed int) over serial
* @param val The short to send to the ODrive
*/
void ODriveArduino::SendShort(int16_t val) {
uint8_t v0 = val & 0xFF;
uint8_t v1 = (val >> 8) & 0xFF;
SendByte(v0);
SendByte(v1);
}
/**
* Sends a command for both motor currents in the form "<1><6>C<i0bytes><i1bytes><checksum>".
* @param current0 Desired current for motor 0
* @param current1 Desired current for motor 1
*/
void ODriveArduino::SetDualCurrent(float current0, float current1) {
// NOTE: Desired current values will be send as their value times 100!
// Ie, 5.6A => 560
// The limit to the multiplier is dictated by 32000/maximum amps
// So if we want 100A maximum value, then the mult could be 320
const int MULTIPLIER = 100;
// constrain the current set points so they don't exceed the limits of int16
current0 = constrain(current0, -30000/MULTIPLIER, 30000/MULTIPLIER);
current1 = constrain(current1, -30000/MULTIPLIER, 30000/MULTIPLIER);
int16_t i0_16 = (current0 * MULTIPLIER);
int16_t i1_16 = (current1 * MULTIPLIER);
// Calculate the checksum based on the 2 current value shorts
uint8_t checkSum = 'C';
checkSum ^= XorShort(i0_16);
checkSum ^= XorShort(i1_16);
// Send off bytes
SendStartByte(); // send start byte
SendByte(6); // payload length
SendByte('C'); // dual current command
SendShort(i0_16);
SendShort(i1_16);
SendByte(checkSum);
}
/**
* Sends a command for a coupled position in the form "<1><6>P<theta_bytes><gamma_bytes><checksum>".
* @param theta Desired theta setpoint
* @param gamma Desired gamma setpoint
*/
void ODriveArduino::SetCoupledPosition(float theta, float gamma) {
const int MULTIPLIER = 1000;
int16_t theta_16 = (theta * MULTIPLIER);
int16_t gamma_16 = (gamma * MULTIPLIER);
// Calculate the checksum based on the 2 current value shorts
uint8_t checkSum = 'P';
checkSum ^= XorShort(theta_16);
checkSum ^= XorShort(gamma_16);
// Send off bytes
SendStartByte(); // send start byte
SendByte(6); // payload length
SendByte('P'); // dual current command
SendShort(theta_16);
SendShort(gamma_16);
SendByte(checkSum);
}
void ODriveArduino::SetCoupledPosition(float sp_theta, float sp_gamma, struct LegGain gains) {
const int POS_MULTIPLIER = 1000;
const int GAIN_MULTIPLIER = 100;
int16_t sp_theta_16 = (sp_theta * POS_MULTIPLIER);
int16_t kp_theta_16 = (gains.kp_theta * GAIN_MULTIPLIER);
int16_t kd_theta_16 = (gains.kd_theta * GAIN_MULTIPLIER);
int16_t sp_gamma_16 = (sp_gamma * POS_MULTIPLIER);
int16_t kp_gamma_16 = (gains.kp_gamma * GAIN_MULTIPLIER);
int16_t kd_gamma_16 = (gains.kd_gamma * GAIN_MULTIPLIER);
// Calculate the checksum based on the 2 current value shorts
uint8_t checkSum = 'S';
checkSum ^= XorShort(sp_theta_16);
checkSum ^= XorShort(kp_theta_16);
checkSum ^= XorShort(kd_theta_16);
checkSum ^= XorShort(sp_gamma_16);
checkSum ^= XorShort(kp_gamma_16);
checkSum ^= XorShort(kd_gamma_16);
// Send off bytes
SendStartByte(); // send start byte
SendByte(14); // payload length
SendByte('S'); // dual current command
SendShort(sp_theta_16);
SendShort(kp_theta_16);
SendShort(kd_theta_16);
SendShort(sp_gamma_16);
SendShort(kp_gamma_16);
SendShort(kd_gamma_16);
SendByte(checkSum);
}
void ODriveArduino::SetCoupledPosition(struct LegGain gains) {
// TODO: correct but where S command doesn't get enough parameters
}
/**
* Send a current command for single axis
* @param motor_number Axis number (0 or 1)
* @param current Float amount of current
*/
void ODriveArduino::SetCurrent(int motor_number, float current) {
SendStartByte(); SendNLLen();
serial_ << "c " << motor_number << " " << current << "\n";
}
/**
* Sends a position command to the ODrive. Once the ODrive gets this command
* then it will use its own PID control to get the motor to that position
* @param motor_number Axis number (0 or 1), Indicate which motor to command
* @param position Desired position in counts
*/
void ODriveArduino::SetPosition(int motor_number, float position) {
SetPosition(motor_number, position, 0.0f, 0.0f);
}
/**
* Sends a position command to the ODrive and set a velocity feedforward gain.
* @param motor_number Axis number (0 or 1), Indicate which motor to command
* @param position Desired position in counts
* @param velocity_feedforward Feed forward term
*/
void ODriveArduino::SetPosition(int motor_number, float position, float velocity_feedforward) {
SetPosition(motor_number, position, velocity_feedforward, 0.0f);
}
/**
* Sends a position command along with velocity feedforward and current feedforward
* @param motor_number Axis number (0 or 1), Indicate which motor to command
* @param position Desired position in counts
* @param velocity_feedforward Velocity feed foward
* @param current_feedforward Current feed forward
*/
void ODriveArduino::SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward) {
SendStartByte(); SendNLLen();
serial_ << "p " << motor_number << " " << position << " " << velocity_feedforward << " " << current_feedforward << "\n";
}
/**
* Sends a velocity command to the ODrive
* @param motor_number Axis number (0 or 1)
* @param velocity Desired velocity
*/
void ODriveArduino::SetVelocity(int motor_number, float velocity) {
SetVelocity(motor_number, velocity, 0.0f);
}
/**
* Sends a velocity command to the ODrive with current feedforward
* @param motor_number Axis number (0 or 1)
* @param velocity Desired velocity
* @param current_feedforward Current feedfoward
*/
void ODriveArduino::SetVelocity(int motor_number, float velocity, float current_feedforward) {
SendStartByte(); SendNLLen();
serial_ << "v " << motor_number << " " << velocity << " " << current_feedforward << "\n";
}
/**
* VERY DUBIOUS / COMPLETELY UNTESTED
*/
/**
* Attempt to read a float sent from the ODrive
* @return Parsed float. TODO: handle error
*/
float ODriveArduino::readFloat() {
return readString().toFloat();
}
/**
* Attempt to read an integer sent from the ODrive
* @return Parsed integer. TODO: handle error
*/
int32_t ODriveArduino::readInt() {
return readString().toInt();
}
/**
* Command a certain desired state for a given axis
* @param axis Axis number (0 or 1)
* @param requested_state Desired state, see state ENUM for viable options
* @param wait Block program until ODrive is back to idle?
* @return Did ODrive successfully reach desired state?
*/
bool ODriveArduino::run_state(int axis, int requested_state, bool wait) {
int timeout_ctr = 100;
SendStartByte(); SendNLLen();
serial_ << "w axis" << axis << ".requested_state " << requested_state << '\n';
if (wait) {
do {
delay(100);
SendStartByte(); SendNLLen();
serial_ << "r axis" << axis << ".current_state\n";
} while (readInt() != AXIS_STATE_IDLE && --timeout_ctr > 0);
}
return timeout_ctr > 0;
}
/**
* Reads a response from the ODrive. Only works if response ends in a '\n'!!
* NOTE: Make sure that the serial thread isn't running concurrently as this function
* or else both codes will try to read from the serial port at the same time!
* @return Received string
*/
String ODriveArduino::readString() {
String str = "";
static const unsigned long timeout = 1000;
unsigned long timeout_start = millis();
for (;;) {
while (!serial_.available()) {
if (millis() - timeout_start >= timeout) {
return str;
}
}
char c = serial_.read();
if (c == '\n') {
break;
}
if (c == 0 || c == 1) { // ignore character if its the start byte or length byte
continue;
}
str += c;
}
return str;
}