Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
odrive.cpp@5:01e1e68309ae, 2018-10-15 (annotated)
- Committer:
- je310
- Date:
- Mon Oct 15 18:30:20 2018 +0000
- Revision:
- 5:01e1e68309ae
- Parent:
- 4:778bc352c47f
testing eigen;
Who changed what in which revision?
User | Revision | Line number | New 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 | 5:01e1e68309ae | 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 | 5:01e1e68309ae | 93 | ss <<"r vbus_voltage"<< '\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 | } |