Miki Nakaone / ODriveLibTest

ODrive用ライブラリ

Dependents:   ODriveTest

ODriveLibTest.cpp@0:c191cf9bf1a3, 2019-09-13 (annotated)

Committer:
m204517
Date:
Fri Sep 13 05:05:14 2019 +0000
Revision:
0:c191cf9bf1a3
Child:
1:dc0c02b4a1b0
operability confirmed

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m204517 0:c191cf9bf1a3 1 /*
m204517 0:c191cf9bf1a3 2 * Modified 19 Aug 2019 by Miki Nakaone : 速度と電流制限の値を変えるのと初期化用のメソッドを追加
m204517 0:c191cf9bf1a3 3 */
m204517 0:c191cf9bf1a3 4
m204517 0:c191cf9bf1a3 5 #include "mbed.h"
m204517 0:c191cf9bf1a3 6 #include <string>
m204517 0:c191cf9bf1a3 7 #include <sstream>
m204517 0:c191cf9bf1a3 8 #include "ODriveLibTest.h"
m204517 0:c191cf9bf1a3 9
m204517 0:c191cf9bf1a3 10 //using namespace std;
m204517 0:c191cf9bf1a3 11
m204517 0:c191cf9bf1a3 12 //using std::string;
m204517 0:c191cf9bf1a3 13
m204517 0:c191cf9bf1a3 14 static const int kMotorOffsetFloat = 2;
m204517 0:c191cf9bf1a3 15 static const int kMotorStrideFloat = 28;
m204517 0:c191cf9bf1a3 16 static const int kMotorOffsetInt32 = 0;
m204517 0:c191cf9bf1a3 17 static const int kMotorStrideInt32 = 4;
m204517 0:c191cf9bf1a3 18 static const int kMotorOffsetBool = 0;
m204517 0:c191cf9bf1a3 19 static const int kMotorStrideBool = 4;
m204517 0:c191cf9bf1a3 20 static const int kMotorOffsetUint16 = 0;
m204517 0:c191cf9bf1a3 21 static const int kMotorStrideUint16 = 2;
m204517 0:c191cf9bf1a3 22
m204517 0:c191cf9bf1a3 23 Timer timeoutTimer;
m204517 0:c191cf9bf1a3 24
m204517 0:c191cf9bf1a3 25 // Print with stream operator
m204517 0:c191cf9bf1a3 26 //template<class T> inline Stream& operator <<(Stream &obj, T arg) { obj.printf(arg); return obj; }
m204517 0:c191cf9bf1a3 27 //template<> inline Stream& operator <<(Stream &obj, float arg) { obj.printf(arg, 4); return obj; }
m204517 0:c191cf9bf1a3 28 /*template<class T> inline Stream& operator <<(Stream &obj, T arg)
m204517 0:c191cf9bf1a3 29 {
m204517 0:c191cf9bf1a3 30 obj.printf(arg);
m204517 0:c191cf9bf1a3 31 return obj;
m204517 0:c191cf9bf1a3 32 }*/
m204517 0:c191cf9bf1a3 33
m204517 0:c191cf9bf1a3 34 ODriveLibTest::ODriveLibTest(Stream& serial)
m204517 0:c191cf9bf1a3 35 : serial_(serial) {}
m204517 0:c191cf9bf1a3 36
m204517 0:c191cf9bf1a3 37 // stoiやstofが使用できないので自作関数 /////////////////////////////////
m204517 0:c191cf9bf1a3 38 float stoi(string str){
m204517 0:c191cf9bf1a3 39 int ret;
m204517 0:c191cf9bf1a3 40 stringstream ss;
m204517 0:c191cf9bf1a3 41 ss << str;
m204517 0:c191cf9bf1a3 42 ss >> ret;
m204517 0:c191cf9bf1a3 43 return ret;
m204517 0:c191cf9bf1a3 44 }
m204517 0:c191cf9bf1a3 45
m204517 0:c191cf9bf1a3 46 float stof(string str){
m204517 0:c191cf9bf1a3 47 float ret;
m204517 0:c191cf9bf1a3 48 stringstream ss;
m204517 0:c191cf9bf1a3 49 ss << str;
m204517 0:c191cf9bf1a3 50 ss >> ret;
m204517 0:c191cf9bf1a3 51 return ret;
m204517 0:c191cf9bf1a3 52 }
m204517 0:c191cf9bf1a3 53
m204517 0:c191cf9bf1a3 54 void ODriveLibTest::SetLimit(int motor_number, float vel_limit, float current_lim) {
m204517 0:c191cf9bf1a3 55 //serial_ << "w axis" << motor_number << ".controller.config.vel_limit " << vel_limit << '\n';
m204517 0:c191cf9bf1a3 56 //serial_ << "w axis" << motor_number << ".motor.config.current_lim " << current_lim << '\n';
m204517 0:c191cf9bf1a3 57 serial_.printf("w axis");
m204517 0:c191cf9bf1a3 58 serial_.printf("%d", motor_number);
m204517 0:c191cf9bf1a3 59 serial_.printf(".controller.config.vel_limit ");
m204517 0:c191cf9bf1a3 60 serial_.printf("%f", vel_limit);
m204517 0:c191cf9bf1a3 61 serial_.printf("\n");
m204517 0:c191cf9bf1a3 62 serial_.printf("w axis");
m204517 0:c191cf9bf1a3 63 serial_.printf("%d", motor_number);
m204517 0:c191cf9bf1a3 64 serial_.printf(".motor.config.current_lim ");
m204517 0:c191cf9bf1a3 65 serial_.printf("%f", current_lim);
m204517 0:c191cf9bf1a3 66 serial_.printf("\n");
m204517 0:c191cf9bf1a3 67
m204517 0:c191cf9bf1a3 68 // まとめて初期化したいとき
m204517 0:c191cf9bf1a3 69 //for (int axis = 0; axis < 2; ++axis) {
m204517 0:c191cf9bf1a3 70 //serial_ << "w axis" << axis << ".controller.config.vel_limit " << vel_limit << '\n';
m204517 0:c191cf9bf1a3 71 //serial_ << "w axis" << axis << ".motor.config.current_lim " << current_lim << '\n';
m204517 0:c191cf9bf1a3 72 //}
m204517 0:c191cf9bf1a3 73 }
m204517 0:c191cf9bf1a3 74
m204517 0:c191cf9bf1a3 75 void ODriveLibTest::ODriveInit(int motor_number) {
m204517 0:c191cf9bf1a3 76 //run_state(motor_number, AXIS_STATE_FULL_CALIBRATION_SEQUENCE, true);
m204517 0:c191cf9bf1a3 77 run_state(motor_number, AXIS_STATE_ENCODER_INDEX_SEARCH, true);
m204517 0:c191cf9bf1a3 78 //　serial_ << "w axis" << motor_number << ".encoder.is_ready " << 1 << '\n';
m204517 0:c191cf9bf1a3 79
m204517 0:c191cf9bf1a3 80 run_state(motor_number, AXIS_STATE_CLOSED_LOOP_CONTROL, false); // don't wait
m204517 0:c191cf9bf1a3 81 }
m204517 0:c191cf9bf1a3 82
m204517 0:c191cf9bf1a3 83 /*void ODriveLibTest::SetPosition(int motor_number, float position) {
m204517 0:c191cf9bf1a3 84 SetPosition(motor_number, position, 0.0f, 0.0f);
m204517 0:c191cf9bf1a3 85 }
m204517 0:c191cf9bf1a3 86
m204517 0:c191cf9bf1a3 87 void ODriveLibTest::SetPosition(int motor_number, float position, float velocity_feedforward) {
m204517 0:c191cf9bf1a3 88 SetPosition(motor_number, position, velocity_feedforward, 0.0f);
m204517 0:c191cf9bf1a3 89 }*/
m204517 0:c191cf9bf1a3 90
m204517 0:c191cf9bf1a3 91 void ODriveLibTest::SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward) {
m204517 0:c191cf9bf1a3 92 //serial_ << "p " << motor_number << " " << position << " " << velocity_feedforward << " " << current_feedforward << "\n";
m204517 0:c191cf9bf1a3 93 serial_.printf("p ");
m204517 0:c191cf9bf1a3 94 serial_.printf("%d", motor_number);
m204517 0:c191cf9bf1a3 95 serial_.printf(" ");
m204517 0:c191cf9bf1a3 96 serial_.printf("%f", position);
m204517 0:c191cf9bf1a3 97 serial_.printf(" ");
m204517 0:c191cf9bf1a3 98 serial_.printf("%f", velocity_feedforward);
m204517 0:c191cf9bf1a3 99 serial_.printf(" ");
m204517 0:c191cf9bf1a3 100 serial_.printf("%f", current_feedforward);
m204517 0:c191cf9bf1a3 101 serial_.printf("\n");
m204517 0:c191cf9bf1a3 102 }
m204517 0:c191cf9bf1a3 103
m204517 0:c191cf9bf1a3 104 void ODriveLibTest::SetVelocity(int motor_number, float velocity) {
m204517 0:c191cf9bf1a3 105 SetVelocity(motor_number, velocity, 0.0f);
m204517 0:c191cf9bf1a3 106 }
m204517 0:c191cf9bf1a3 107
m204517 0:c191cf9bf1a3 108 void ODriveLibTest::SetVelocity(int motor_number, float velocity, float current_feedforward) {
m204517 0:c191cf9bf1a3 109 //serial_ << "v " << motor_number << " " << velocity << " " << current_feedforward << "\n";
m204517 0:c191cf9bf1a3 110 serial_.printf("v ");
m204517 0:c191cf9bf1a3 111 serial_.printf("%d", motor_number);
m204517 0:c191cf9bf1a3 112 serial_.printf(" ");
m204517 0:c191cf9bf1a3 113 serial_.printf("%f", velocity);
m204517 0:c191cf9bf1a3 114 serial_.printf(" ");
m204517 0:c191cf9bf1a3 115 serial_.printf("%f", current_feedforward);
m204517 0:c191cf9bf1a3 116 serial_.printf("\n");
m204517 0:c191cf9bf1a3 117 }
m204517 0:c191cf9bf1a3 118
m204517 0:c191cf9bf1a3 119 void ODriveLibTest::SetCurrent(int motor_number, float current) {
m204517 0:c191cf9bf1a3 120 //serial_ << "c " << motor_number << " " << current << "\n";
m204517 0:c191cf9bf1a3 121 serial_.printf("c ");
m204517 0:c191cf9bf1a3 122 serial_.printf("%d", motor_number);
m204517 0:c191cf9bf1a3 123 serial_.printf(" ");
m204517 0:c191cf9bf1a3 124 serial_.printf("%f", current);
m204517 0:c191cf9bf1a3 125 serial_.printf("\n");
m204517 0:c191cf9bf1a3 126 }
m204517 0:c191cf9bf1a3 127
m204517 0:c191cf9bf1a3 128 void ODriveLibTest::TrapezoidalMove(int motor_number, float position){
m204517 0:c191cf9bf1a3 129 //serial_ << "t " << motor_number << " " << position << "\n";
m204517 0:c191cf9bf1a3 130 serial_.printf("c ");
m204517 0:c191cf9bf1a3 131 serial_.printf("%d", motor_number);
m204517 0:c191cf9bf1a3 132 serial_.printf(" ");
m204517 0:c191cf9bf1a3 133 serial_.printf("%f", position);
m204517 0:c191cf9bf1a3 134 serial_.printf("\n");
m204517 0:c191cf9bf1a3 135 }
m204517 0:c191cf9bf1a3 136
m204517 0:c191cf9bf1a3 137 float ODriveLibTest::readFloat() {
m204517 0:c191cf9bf1a3 138 return stof(readString());
m204517 0:c191cf9bf1a3 139 }
m204517 0:c191cf9bf1a3 140
m204517 0:c191cf9bf1a3 141 float ODriveLibTest::GetVelocity(int motor_number){
m204517 0:c191cf9bf1a3 142 //serial_<< "r axis" << motor_number << ".encoder.vel_estimate\n";
m204517 0:c191cf9bf1a3 143 serial_.printf("r axis");
m204517 0:c191cf9bf1a3 144 serial_.printf("%d", motor_number);
m204517 0:c191cf9bf1a3 145 serial_.printf(".encoder.vel_estimate\n");
m204517 0:c191cf9bf1a3 146 return ODriveLibTest::readFloat();
m204517 0:c191cf9bf1a3 147 }
m204517 0:c191cf9bf1a3 148
m204517 0:c191cf9bf1a3 149 int32_t ODriveLibTest::readInt() {
m204517 0:c191cf9bf1a3 150 return stoi(readString());
m204517 0:c191cf9bf1a3 151 }
m204517 0:c191cf9bf1a3 152
m204517 0:c191cf9bf1a3 153 bool ODriveLibTest::run_state(int axis, int requested_state, bool wait) {
m204517 0:c191cf9bf1a3 154 int timeout_ctr = 100;
m204517 0:c191cf9bf1a3 155 //serial_ << "w axis" << axis << ".requested_state " << requested_state << '\n';
m204517 0:c191cf9bf1a3 156 serial_.printf("w axis");
m204517 0:c191cf9bf1a3 157 serial_.printf("%d", axis);
m204517 0:c191cf9bf1a3 158 serial_.printf(".requested_state ");
m204517 0:c191cf9bf1a3 159 serial_.printf("%d", requested_state);
m204517 0:c191cf9bf1a3 160 serial_.printf("\n");
m204517 0:c191cf9bf1a3 161 if (wait) {
m204517 0:c191cf9bf1a3 162 do {
m204517 0:c191cf9bf1a3 163 wait_ms(100);
m204517 0:c191cf9bf1a3 164 //serial_ << "r axis" << axis << ".current_state\n";
m204517 0:c191cf9bf1a3 165 serial_.printf("r axis");
m204517 0:c191cf9bf1a3 166 serial_.printf("%d", axis);
m204517 0:c191cf9bf1a3 167 serial_.printf(".current_state\n");
m204517 0:c191cf9bf1a3 168 } while (readInt() != AXIS_STATE_IDLE && --timeout_ctr > 0);
m204517 0:c191cf9bf1a3 169 }
m204517 0:c191cf9bf1a3 170
m204517 0:c191cf9bf1a3 171 return timeout_ctr > 0;
m204517 0:c191cf9bf1a3 172 }
m204517 0:c191cf9bf1a3 173
m204517 0:c191cf9bf1a3 174 string ODriveLibTest::readString() {
m204517 0:c191cf9bf1a3 175 string str = "";
m204517 0:c191cf9bf1a3 176
m204517 0:c191cf9bf1a3 177 static const unsigned long timeout = 1000;
m204517 0:c191cf9bf1a3 178 timeoutTimer.start();
m204517 0:c191cf9bf1a3 179 for(;;){ // ;;は無限ループらしい
m204517 0:c191cf9bf1a3 180 while(!serial_.readable()){
m204517 0:c191cf9bf1a3 181 if( timeoutTimer.read_ms() >= timeout ){
m204517 0:c191cf9bf1a3 182 return str;
m204517 0:c191cf9bf1a3 183 }
m204517 0:c191cf9bf1a3 184 }
m204517 0:c191cf9bf1a3 185 char c = serial_.getc();
m204517 0:c191cf9bf1a3 186 if( c == '\n' )
m204517 0:c191cf9bf1a3 187 break;
m204517 0:c191cf9bf1a3 188 str += c;
m204517 0:c191cf9bf1a3 189 }
m204517 0:c191cf9bf1a3 190 timeoutTimer.stop();
m204517 0:c191cf9bf1a3 191 timeoutTimer.reset();
m204517 0:c191cf9bf1a3 192 return str;
m204517 0:c191cf9bf1a3 193 }