File content as of revision 0:c191cf9bf1a3:

/*
 *  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;

// Print with stream operator
//template<class T> inline Stream& operator <<(Stream &obj,     T arg) { obj.printf(arg);    return obj; }
//template<>        inline Stream& operator <<(Stream &obj, float arg) { obj.printf(arg, 4); return obj; }
/*template<class T> inline Stream& operator <<(Stream &obj,     T arg)
{
    obj.printf(arg);
    return obj;
}*/

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_ << "w axis" << motor_number << ".controller.config.vel_limit " << vel_limit << '\n';
    //serial_ << "w axis" << motor_number << ".motor.config.current_lim " << current_lim << '\n';
    serial_.printf("w axis");
    serial_.printf("%d", motor_number);
    serial_.printf(".controller.config.vel_limit ");
    serial_.printf("%f", vel_limit);
    serial_.printf("\n");
    serial_.printf("w axis");
    serial_.printf("%d", motor_number);
    serial_.printf(".motor.config.current_lim ");
    serial_.printf("%f", current_lim);
    serial_.printf("\n");

    // まとめて初期化したいとき
    //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);
    //　serial_ << "w axis" << motor_number << ".encoder.is_ready " << 1 << '\n';
    
    run_state(motor_number, AXIS_STATE_CLOSED_LOOP_CONTROL, false); // don't wait
}

/*void ODriveLibTest::SetPosition(int motor_number, float position) {
    SetPosition(motor_number, position, 0.0f, 0.0f);
}

void ODriveLibTest::SetPosition(int motor_number, float position, float velocity_feedforward) {
    SetPosition(motor_number, position, velocity_feedforward, 0.0f);
}*/

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

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_ << "v " << motor_number  << " " << velocity << " " << current_feedforward << "\n";
    serial_.printf("v ");
    serial_.printf("%d", motor_number);
    serial_.printf(" ");
    serial_.printf("%f", velocity);
    serial_.printf(" ");
    serial_.printf("%f", current_feedforward);
    serial_.printf("\n");
}

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

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

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

float ODriveLibTest::GetVelocity(int motor_number){
    //serial_<< "r axis" << motor_number << ".encoder.vel_estimate\n";
    serial_.printf("r axis");
    serial_.printf("%d", motor_number);
    serial_.printf(".encoder.vel_estimate\n");
    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_ << "w axis" << axis << ".requested_state " << requested_state << '\n';
    serial_.printf("w axis");
    serial_.printf("%d", axis);
    serial_.printf(".requested_state ");
    serial_.printf("%d", requested_state);
    serial_.printf("\n");
    if (wait) {
        do {
            wait_ms(100);
            //serial_ << "r axis" << axis << ".current_state\n";
            serial_.printf("r axis");
            serial_.printf("%d", axis);
            serial_.printf(".current_state\n");
        } 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;
}