Revision 0:c191cf9bf1a3, committed 2019-09-13

Comitter:

m204517

Date:

Fri Sep 13 05:05:14 2019 +0000

Child:

1:dc0c02b4a1b0

Commit message:

operability confirmed

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ODriveLibTest.cpp	Fri Sep 13 05:05:14 2019 +0000
@@ -0,0 +1,193 @@
+/*
+ *  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;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ODriveLibTest.h	Fri Sep 13 05:05:14 2019 +0000
@@ -0,0 +1,50 @@
+
+#ifndef ODriveLibTest_h
+#define ODriveLibTest_h
+
+#include "mbed.h"
+#include "string"
+
+//using std::string;
+
+class ODriveLibTest {
+public:
+    enum AxisState_t {
+        AXIS_STATE_UNDEFINED = 0,           //<! will fall through to idle
+        AXIS_STATE_IDLE = 1,                //<! disable PWM and do nothing
+        AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
+        AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3,   //<! run all calibration procedures, then idle
+        AXIS_STATE_MOTOR_CALIBRATION = 4,   //<! run motor calibration
+        AXIS_STATE_SENSORLESS_CONTROL = 5,  //<! run sensorless control
+        AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
+        AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
+        AXIS_STATE_CLOSED_LOOP_CONTROL = 8  //<! run closed loop control
+    };
+
+    ODriveLibTest(Stream& serial);
+
+    // Commands
+    void SetLimit(int motor_number, float vel_limit, float current_lim);
+    void ODriveInit(int motor_number);
+    //void SetPosition(int motor_number, float position);
+    //void SetPosition(int motor_number, float position, float velocity_feedforward);
+    void SetPosition(int motor_number, float position, float velocity_feedforward = 0.0f, float current_feedforward = 0.0f);
+    void SetVelocity(int motor_number, float velocity);
+    void SetVelocity(int motor_number, float velocity, float current_feedforward);
+    void SetCurrent(int motor_number, float current);
+    void TrapezoidalMove(int motor_number, float position);
+    // Getters
+    float GetVelocity(int motor_number);
+    // General params
+    float readFloat();
+    int32_t readInt();
+
+    // State helper
+    bool run_state(int axis, int requested_state, bool wait);
+private:
+    string readString();
+
+    Stream& serial_;
+};
+
+#endif //ODriveArduino_h
\ No newline at end of file