Miki Nakaone / ODriveLibTest

ODrive用ライブラリ

Dependents:   ODriveTest

ODriveLibTest.cpp@1:dc0c02b4a1b0, 2019-09-13 (annotated)

Committer:
m204517
Date:
Fri Sep 13 05:49:54 2019 +0000
Revision:
1:dc0c02b4a1b0
Parent:
0:c191cf9bf1a3 
modification

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 //using std::string;
m204517 0:c191cf9bf1a3 12
m204517 0:c191cf9bf1a3 13 static const int kMotorOffsetFloat = 2;
m204517 0:c191cf9bf1a3 14 static const int kMotorStrideFloat = 28;
m204517 0:c191cf9bf1a3 15 static const int kMotorOffsetInt32 = 0;
m204517 0:c191cf9bf1a3 16 static const int kMotorStrideInt32 = 4;
m204517 0:c191cf9bf1a3 17 static const int kMotorOffsetBool = 0;
m204517 0:c191cf9bf1a3 18 static const int kMotorStrideBool = 4;
m204517 0:c191cf9bf1a3 19 static const int kMotorOffsetUint16 = 0;
m204517 0:c191cf9bf1a3 20 static const int kMotorStrideUint16 = 2;
m204517 0:c191cf9bf1a3 21
m204517 0:c191cf9bf1a3 22 Timer timeoutTimer;
m204517 0:c191cf9bf1a3 23
m204517 0:c191cf9bf1a3 24 ODriveLibTest::ODriveLibTest(Stream& serial)
m204517 0:c191cf9bf1a3 25 : serial_(serial) {}
m204517 0:c191cf9bf1a3 26
m204517 0:c191cf9bf1a3 27 // stoiやstofが使用できないので自作関数 /////////////////////////////////
m204517 0:c191cf9bf1a3 28 float stoi(string str){
m204517 0:c191cf9bf1a3 29 int ret;
m204517 0:c191cf9bf1a3 30 stringstream ss;
m204517 0:c191cf9bf1a3 31 ss << str;
m204517 0:c191cf9bf1a3 32 ss >> ret;
m204517 0:c191cf9bf1a3 33 return ret;
m204517 0:c191cf9bf1a3 34 }
m204517 0:c191cf9bf1a3 35
m204517 0:c191cf9bf1a3 36 float stof(string str){
m204517 0:c191cf9bf1a3 37 float ret;
m204517 0:c191cf9bf1a3 38 stringstream ss;
m204517 0:c191cf9bf1a3 39 ss << str;
m204517 0:c191cf9bf1a3 40 ss >> ret;
m204517 0:c191cf9bf1a3 41 return ret;
m204517 0:c191cf9bf1a3 42 }
m204517 0:c191cf9bf1a3 43
m204517 0:c191cf9bf1a3 44 void ODriveLibTest::SetLimit(int motor_number, float vel_limit, float current_lim) {
m204517 1:dc0c02b4a1b0 45 serial_.printf("w axis%d.controller.config.vel_limit %f\n", motor_number, vel_limit);
m204517 1:dc0c02b4a1b0 46 serial_.printf("w axis%d.motor.config.current_lim %f\n", motor_number, current_lim);
m204517 0:c191cf9bf1a3 47
m204517 0:c191cf9bf1a3 48 // まとめて初期化したいとき
m204517 0:c191cf9bf1a3 49 //for (int axis = 0; axis < 2; ++axis) {
m204517 0:c191cf9bf1a3 50 //serial_ << "w axis" << axis << ".controller.config.vel_limit " << vel_limit << '\n';
m204517 0:c191cf9bf1a3 51 //serial_ << "w axis" << axis << ".motor.config.current_lim " << current_lim << '\n';
m204517 0:c191cf9bf1a3 52 //}
m204517 0:c191cf9bf1a3 53 }
m204517 0:c191cf9bf1a3 54
m204517 0:c191cf9bf1a3 55 void ODriveLibTest::ODriveInit(int motor_number) {
m204517 0:c191cf9bf1a3 56 //run_state(motor_number, AXIS_STATE_FULL_CALIBRATION_SEQUENCE, true);
m204517 0:c191cf9bf1a3 57 run_state(motor_number, AXIS_STATE_ENCODER_INDEX_SEARCH, true);
m204517 0:c191cf9bf1a3 58
m204517 0:c191cf9bf1a3 59 run_state(motor_number, AXIS_STATE_CLOSED_LOOP_CONTROL, false); // don't wait
m204517 0:c191cf9bf1a3 60 }
m204517 0:c191cf9bf1a3 61
m204517 0:c191cf9bf1a3 62 void ODriveLibTest::SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward) {
m204517 1:dc0c02b4a1b0 63 serial_.printf("p %d %f %f %f\n", motor_number, position, velocity_feedforward, current_feedforward);
m204517 0:c191cf9bf1a3 64 }
m204517 0:c191cf9bf1a3 65
m204517 0:c191cf9bf1a3 66 void ODriveLibTest::SetVelocity(int motor_number, float velocity) {
m204517 0:c191cf9bf1a3 67 SetVelocity(motor_number, velocity, 0.0f);
m204517 0:c191cf9bf1a3 68 }
m204517 0:c191cf9bf1a3 69
m204517 0:c191cf9bf1a3 70 void ODriveLibTest::SetVelocity(int motor_number, float velocity, float current_feedforward) {
m204517 1:dc0c02b4a1b0 71 serial_.printf("v %d %f %f\n", motor_number, velocity, current_feedforward);
m204517 0:c191cf9bf1a3 72 }
m204517 0:c191cf9bf1a3 73
m204517 0:c191cf9bf1a3 74 void ODriveLibTest::SetCurrent(int motor_number, float current) {
m204517 1:dc0c02b4a1b0 75 serial_.printf("c %d %f\n", motor_number, current);
m204517 0:c191cf9bf1a3 76 }
m204517 0:c191cf9bf1a3 77
m204517 0:c191cf9bf1a3 78 void ODriveLibTest::TrapezoidalMove(int motor_number, float position){
m204517 1:dc0c02b4a1b0 79 serial_.printf("c %d %f\n", motor_number, position);
m204517 0:c191cf9bf1a3 80 }
m204517 0:c191cf9bf1a3 81
m204517 0:c191cf9bf1a3 82 float ODriveLibTest::readFloat() {
m204517 0:c191cf9bf1a3 83 return stof(readString());
m204517 0:c191cf9bf1a3 84 }
m204517 0:c191cf9bf1a3 85
m204517 0:c191cf9bf1a3 86 float ODriveLibTest::GetVelocity(int motor_number){
m204517 1:dc0c02b4a1b0 87 serial_.printf("r axis%d.encoder.vel_estimate\n", motor_number);
m204517 0:c191cf9bf1a3 88 return ODriveLibTest::readFloat();
m204517 0:c191cf9bf1a3 89 }
m204517 0:c191cf9bf1a3 90
m204517 0:c191cf9bf1a3 91 int32_t ODriveLibTest::readInt() {
m204517 0:c191cf9bf1a3 92 return stoi(readString());
m204517 0:c191cf9bf1a3 93 }
m204517 0:c191cf9bf1a3 94
m204517 0:c191cf9bf1a3 95 bool ODriveLibTest::run_state(int axis, int requested_state, bool wait) {
m204517 0:c191cf9bf1a3 96 int timeout_ctr = 100;
m204517 1:dc0c02b4a1b0 97 serial_.printf("w axis%d.requested_state %d\n", axis, requested_state);
m204517 0:c191cf9bf1a3 98 if (wait) {
m204517 0:c191cf9bf1a3 99 do {
m204517 0:c191cf9bf1a3 100 wait_ms(100);
m204517 1:dc0c02b4a1b0 101 serial_.printf("r axis%d.current_state\n", axis);
m204517 0:c191cf9bf1a3 102 } while (readInt() != AXIS_STATE_IDLE && --timeout_ctr > 0);
m204517 0:c191cf9bf1a3 103 }
m204517 0:c191cf9bf1a3 104
m204517 0:c191cf9bf1a3 105 return timeout_ctr > 0;
m204517 0:c191cf9bf1a3 106 }
m204517 0:c191cf9bf1a3 107
m204517 0:c191cf9bf1a3 108 string ODriveLibTest::readString() {
m204517 0:c191cf9bf1a3 109 string str = "";
m204517 0:c191cf9bf1a3 110
m204517 0:c191cf9bf1a3 111 static const unsigned long timeout = 1000;
m204517 0:c191cf9bf1a3 112 timeoutTimer.start();
m204517 0:c191cf9bf1a3 113 for(;;){ // ;;は無限ループらしい
m204517 0:c191cf9bf1a3 114 while(!serial_.readable()){
m204517 0:c191cf9bf1a3 115 if( timeoutTimer.read_ms() >= timeout ){
m204517 0:c191cf9bf1a3 116 return str;
m204517 0:c191cf9bf1a3 117 }
m204517 0:c191cf9bf1a3 118 }
m204517 0:c191cf9bf1a3 119 char c = serial_.getc();
m204517 0:c191cf9bf1a3 120 if( c == '\n' )
m204517 0:c191cf9bf1a3 121 break;
m204517 0:c191cf9bf1a3 122 str += c;
m204517 0:c191cf9bf1a3 123 }
m204517 0:c191cf9bf1a3 124 timeoutTimer.stop();
m204517 0:c191cf9bf1a3 125 timeoutTimer.reset();
m204517 0:c191cf9bf1a3 126 return str;
m204517 0:c191cf9bf1a3 127 }