joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

odrive.cpp@4:778bc352c47f, 2018-10-07 (annotated)

Committer:
je310
Date:
Sun Oct 07 19:40:12 2018 +0000
Revision:
4:778bc352c47f
Parent:
3:10fa3102c2d7
Child:
5:01e1e68309ae
can work smoothly, though voltage reading not working;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
je310 3:10fa3102c2d7 1 #include "odrive.h"
je310 4:778bc352c47f 2 extern BufferedSerial buffered_pc;
je310 3:10fa3102c2d7 3 static const int kMotorOffsetFloat = 2;
je310 3:10fa3102c2d7 4 static const int kMotorStrideFloat = 28;
je310 3:10fa3102c2d7 5 static const int kMotorOffsetInt32 = 0;
je310 3:10fa3102c2d7 6 static const int kMotorStrideInt32 = 4;
je310 3:10fa3102c2d7 7 static const int kMotorOffsetBool = 0;
je310 3:10fa3102c2d7 8 static const int kMotorStrideBool = 4;
je310 3:10fa3102c2d7 9 static const int kMotorOffsetUint16 = 0;
je310 3:10fa3102c2d7 10 static const int kMotorStrideUint16 = 2;
je310 3:10fa3102c2d7 11
je310 3:10fa3102c2d7 12
je310 3:10fa3102c2d7 13 ODrive::ODrive(BufferedSerial& serial)
je310 3:10fa3102c2d7 14 : serial_(serial) {}
je310 3:10fa3102c2d7 15
je310 3:10fa3102c2d7 16 void ODrive::SetPosition(int motor_number, float position) {
je310 3:10fa3102c2d7 17 SetPosition(motor_number, position, 0.0f, 0.0f);
je310 3:10fa3102c2d7 18 }
je310 3:10fa3102c2d7 19
je310 3:10fa3102c2d7 20 void ODrive::SetPosition(int motor_number, float position, float velocity_feedforward) {
je310 3:10fa3102c2d7 21 SetPosition(motor_number, position, velocity_feedforward, 0.0f);
je310 3:10fa3102c2d7 22 }
je310 3:10fa3102c2d7 23
je310 3:10fa3102c2d7 24 void ODrive::SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward) {
je310 3:10fa3102c2d7 25 std::stringstream ss;
je310 3:10fa3102c2d7 26 ss << "p " << motor_number << " " << position << " " << velocity_feedforward << " " << current_feedforward << "\n";
je310 3:10fa3102c2d7 27 serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
je310 3:10fa3102c2d7 28 }
je310 3:10fa3102c2d7 29
je310 3:10fa3102c2d7 30 void ODrive::SetVelocity(int motor_number, float velocity) {
je310 3:10fa3102c2d7 31 SetVelocity(motor_number, velocity, 0.0f);
je310 3:10fa3102c2d7 32 }
je310 3:10fa3102c2d7 33
je310 3:10fa3102c2d7 34 void ODrive::SetVelocity(int motor_number, float velocity, float current_feedforward) {
je310 3:10fa3102c2d7 35 std::stringstream ss;
je310 3:10fa3102c2d7 36 ss << "v " << motor_number << " " << velocity << " " << current_feedforward << "\n";
je310 3:10fa3102c2d7 37 serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
je310 3:10fa3102c2d7 38 }
je310 3:10fa3102c2d7 39
je310 3:10fa3102c2d7 40 float ODrive::readFloat() {
je310 3:10fa3102c2d7 41 return atof(readString().c_str());
je310 3:10fa3102c2d7 42 }
je310 3:10fa3102c2d7 43
je310 3:10fa3102c2d7 44 int32_t ODrive::readInt() {
je310 3:10fa3102c2d7 45 return atoi(readString().c_str());
je310 3:10fa3102c2d7 46 }
je310 3:10fa3102c2d7 47
je310 3:10fa3102c2d7 48 void ODrive::FinishedSending(){
je310 3:10fa3102c2d7 49
je310 3:10fa3102c2d7 50 }
je310 3:10fa3102c2d7 51
je310 3:10fa3102c2d7 52 bool ODrive::run_state(int axis, int requested_state, bool wait) {
je310 3:10fa3102c2d7 53 int timeout_ctr = 100;
je310 3:10fa3102c2d7 54 std::stringstream ss;
je310 3:10fa3102c2d7 55 ss << "w axis" << axis << ".requested_state " << requested_state << '\n';
je310 3:10fa3102c2d7 56 serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
je310 3:10fa3102c2d7 57 if (wait) {
je310 3:10fa3102c2d7 58 do {
je310 3:10fa3102c2d7 59 Thread::wait(100);
je310 3:10fa3102c2d7 60 std::stringstream ss2;
je310 3:10fa3102c2d7 61 ss2 << "r axis" << axis << ".current_state\n";
je310 3:10fa3102c2d7 62 serial_.write((const uint8_t *)ss2.str().c_str(),ss2.str().length());
je310 3:10fa3102c2d7 63 } while (readInt() != AXIS_STATE_IDLE && --timeout_ctr > 0);
je310 3:10fa3102c2d7 64 }
je310 3:10fa3102c2d7 65
je310 3:10fa3102c2d7 66 return timeout_ctr > 0;
je310 3:10fa3102c2d7 67 }
je310 3:10fa3102c2d7 68
je310 3:10fa3102c2d7 69 std::string ODrive::readString() {
je310 3:10fa3102c2d7 70 Timer timer;
je310 3:10fa3102c2d7 71 std::string str = "";
je310 3:10fa3102c2d7 72 static const unsigned long timeout = 1000;
je310 4:778bc352c47f 73 timer.reset();
je310 3:10fa3102c2d7 74 timer.start();
je310 3:10fa3102c2d7 75 for (;;) {
je310 3:10fa3102c2d7 76 while (!serial_.readable ()) {
je310 3:10fa3102c2d7 77 if (timer.read_ms() >= timeout) {
je310 3:10fa3102c2d7 78 return str;
je310 3:10fa3102c2d7 79 }
je310 3:10fa3102c2d7 80 }
je310 3:10fa3102c2d7 81 char c = serial_.getc ();
je310 3:10fa3102c2d7 82 if (c == '\n')
je310 3:10fa3102c2d7 83 break;
je310 3:10fa3102c2d7 84 str += c;
je310 3:10fa3102c2d7 85 }
je310 3:10fa3102c2d7 86 timer.stop();
je310 4:778bc352c47f 87 buffered_pc.printf(str.c_str());
je310 3:10fa3102c2d7 88 return str;
je310 4:778bc352c47f 89 }
je310 4:778bc352c47f 90
je310 4:778bc352c47f 91 float ODrive::readBattery(){
je310 4:778bc352c47f 92 std::stringstream ss;
je310 4:778bc352c47f 93 ss <<"r vbus_voltage\n" << '\n';
je310 4:778bc352c47f 94 serial_.write((const uint8_t *)ss.str().c_str(),ss.str().length());
je310 4:778bc352c47f 95 return readFloat();
je310 4:778bc352c47f 96
je310 4:778bc352c47f 97 }