File content as of revision 1:dc0c02b4a1b0:

/*
 *  Modified 19 Aug 2019 by Miki Nakaone : 速度と電流制限の値を変えるのと初期化用のメソッドを追加
 */

#include "mbed.h"
#include <string>
#include <sstream>
#include "ODriveLibTest.h"

//using namespace std;
//using std::string;

static const int kMotorOffsetFloat = 2;
static const int kMotorStrideFloat = 28;
static const int kMotorOffsetInt32 = 0;
static const int kMotorStrideInt32 = 4;
static const int kMotorOffsetBool = 0;
static const int kMotorStrideBool = 4;
static const int kMotorOffsetUint16 = 0;
static const int kMotorStrideUint16 = 2;

Timer timeoutTimer;

ODriveLibTest::ODriveLibTest(Stream& serial)
    : serial_(serial) {}

// stoiやstofが使用できないので自作関数 /////////////////////////////////
float stoi(string str){
    int ret;
    stringstream ss;
    ss << str;
    ss >> ret;
    return ret;
}

float stof(string str){
    float ret;
    stringstream ss;
    ss << str;
    ss >> ret;
    return ret;
}

void ODriveLibTest::SetLimit(int motor_number, float vel_limit, float current_lim) {
    serial_.printf("w axis%d.controller.config.vel_limit %f\n", motor_number, vel_limit);
    serial_.printf("w axis%d.motor.config.current_lim %f\n", motor_number, current_lim);

    // まとめて初期化したいとき
    //for (int axis = 0; axis < 2; ++axis) {
        //serial_ << "w axis" << axis << ".controller.config.vel_limit " << vel_limit << '\n';
        //serial_ << "w axis" << axis << ".motor.config.current_lim " << current_lim << '\n';
    //}
}

void ODriveLibTest::ODriveInit(int motor_number) {
    //run_state(motor_number, AXIS_STATE_FULL_CALIBRATION_SEQUENCE, true);
    run_state(motor_number, AXIS_STATE_ENCODER_INDEX_SEARCH, true);
    
    run_state(motor_number, AXIS_STATE_CLOSED_LOOP_CONTROL, false); // don't wait
}

void ODriveLibTest::SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward) {
    serial_.printf("p %d %f %f %f\n", motor_number, position, velocity_feedforward, current_feedforward);
}

void ODriveLibTest::SetVelocity(int motor_number, float velocity) {
    SetVelocity(motor_number, velocity, 0.0f);
}

void ODriveLibTest::SetVelocity(int motor_number, float velocity, float current_feedforward) {
    serial_.printf("v %d %f %f\n", motor_number, velocity, current_feedforward);
}

void ODriveLibTest::SetCurrent(int motor_number, float current) {
    serial_.printf("c %d %f\n", motor_number, current);
}

void ODriveLibTest::TrapezoidalMove(int motor_number, float position){
    serial_.printf("c %d %f\n", motor_number, position);
}

float ODriveLibTest::readFloat() {
    return stof(readString());
}

float ODriveLibTest::GetVelocity(int motor_number){
    serial_.printf("r axis%d.encoder.vel_estimate\n", motor_number);
    return ODriveLibTest::readFloat();
}

int32_t ODriveLibTest::readInt() {
    return stoi(readString());
}

bool ODriveLibTest::run_state(int axis, int requested_state, bool wait) {
    int timeout_ctr = 100;
    serial_.printf("w axis%d.requested_state %d\n", axis, requested_state);
    if (wait) {
        do {
            wait_ms(100);
            serial_.printf("r axis%d.current_state\n", axis);
        } while (readInt() != AXIS_STATE_IDLE && --timeout_ctr > 0);
    }

    return timeout_ctr > 0;
}

string ODriveLibTest::readString() {
    string str = "";
    
    static const unsigned long timeout = 1000;
    timeoutTimer.start();
    for(;;){ // ;;は無限ループらしい
        while(!serial_.readable()){
            if( timeoutTimer.read_ms() >= timeout ){
                return str;
            }
        }
        char c = serial_.getc();
        if( c == '\n' )
            break;
        str += c;
    }
    timeoutTimer.stop();
    timeoutTimer.reset();
    return str;
}