Diff: ODriveLibTest.cpp

Revision:

1:dc0c02b4a1b0

Parent:

0:c191cf9bf1a3

--- a/ODriveLibTest.cpp	Fri Sep 13 05:05:14 2019 +0000
+++ b/ODriveLibTest.cpp	Fri Sep 13 05:49:54 2019 +0000
@@ -8,7 +8,6 @@
 #include "ODriveLibTest.h"
 
 //using namespace std;
-
 //using std::string;
 
 static const int kMotorOffsetFloat = 2;
@@ -22,15 +21,6 @@
 
 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) {}
 
@@ -52,18 +42,8 @@
 }
 
 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");
+    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) {
@@ -75,30 +55,12 @@
 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");
+    serial_.printf("p %d %f %f %f\n", motor_number, position, velocity_feedforward, current_feedforward);
 }
 
 void ODriveLibTest::SetVelocity(int motor_number, float velocity) {
@@ -106,32 +68,15 @@
 }
 
 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");
+    serial_.printf("v %d %f %f\n", motor_number, velocity, current_feedforward);
 }
 
 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");
+    serial_.printf("c %d %f\n", motor_number, current);
 }
 
 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");
+    serial_.printf("c %d %f\n", motor_number, position);
 }
 
 float ODriveLibTest::readFloat() {
@@ -139,10 +84,7 @@
 }
 
 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");
+    serial_.printf("r axis%d.encoder.vel_estimate\n", motor_number);
     return ODriveLibTest::readFloat();
 }
 
@@ -152,19 +94,11 @@
 
 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");
+    serial_.printf("w axis%d.requested_state %d\n", axis, requested_state);
     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");
+            serial_.printf("r axis%d.current_state\n", axis);
         } while (readInt() != AXIS_STATE_IDLE && --timeout_ctr > 0);
     }