Diff: ODriveLibTest.cpp

Revision:

0:c191cf9bf1a3

Child:

1:dc0c02b4a1b0

diff -r 000000000000 -r c191cf9bf1a3 ODriveLibTest.cpp
--- /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