Diff: main.cpp

Revision:

1:2823a39f70a9

Parent:

0:c23e915f255b

Child:

2:f630aff31d27

--- a/main.cpp	Sun Aug 05 13:15:56 2018 +0000
+++ b/main.cpp	Tue Aug 07 08:47:59 2018 +0000
@@ -22,7 +22,7 @@
 
 // Timer
 Ticker timer1;
-float ITR_time1 = 10000.0;  // unit：us
+float ITR_time1 = 4000.0;  // unit：us
 float Ts = ITR_time1/1000000;
 uint8_t flag;
 
@@ -42,6 +42,7 @@
 // Dynamixel
 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
 int servo_cmd;
+int row_cmd;
 int D_angle = 0;
 int D_angle_dif = 0;
 int D_Angle;
@@ -53,10 +54,12 @@
 float ks = 2.6393*2;  //spring constant
 //int angle_dif = 0;
 float torque_ref = 0.0;
+//float friction = 0.0f;
 float friction = 0.18f;
-float rate = 0.8;
+float rate = 0.5;
 //float friction = 0.0f;
 //float check = 0.0f;
+float Angle_Dif;
 
 // function
 void init_UART();
@@ -82,7 +85,7 @@
     wait_ms(500);
 
     led2 = 1;
-    led2f = 0;
+//    led2f = 0;
     LED = 0; // lighten
 
     init_TIMER();
@@ -95,7 +98,7 @@
             find_torque();
             motor_pid.Compute(torque_ref, torque_measured);
             PIDout = motor_pid.output;
-            servo_cmd = PIDout*121.8f;   // 1023/8.4Nm = 121.7857
+            servo_cmd = -PIDout*121.8f;   // 1023/8.4Nm = 121.7857
 
             if (servo_cmd > 0) {
                 servo_cmd = servo_cmd + ((-torque_ref)*rate+friction)*121.8f;
@@ -106,7 +109,14 @@
                 if (servo_cmd >= 2047)
                     servo_cmd = 2047;
             }
-//    dynamixelClass.torque(SERVO_ID, servo_cmd);
+
+            if (servo_cmd >= 1023) {
+                row_cmd = -(servo_cmd-1023);
+            } else {
+                row_cmd = servo_cmd;
+            }
+
+//            dynamixelClass.torque(SERVO_ID, servo_cmd);
             uart_tx();
             flag = 0;
         }
@@ -132,12 +142,12 @@
 void timer1_ITR()
 {
     flag = 1;
-    if (led2f == 5) {
-
-        led2f = 0;
-    } else {
-        led2f++;
-    }
+//    if (led2f == 5) {
+//
+//        led2f = 0;
+//    } else {
+//        led2f++;
+//    }
 }
 
 void uart_tx()
@@ -150,15 +160,16 @@
     splitter s6;
     splitter s7;
 
-    s1.j = angle;
+    s1.j = 1;
     s2.j = Angle;
-    s3.j = 2;
+    s3.j = D_Angle;
+//    s3.j = servo_cmd;
 //    s4.j = 1;
 //    s5.j = 3;
-    s4.j = D_angle;
-    s5.j = D_Angle;
-    s6.j = torque_measured*1000;
-    s7.j = 3;
+    s4.j = angle_difference;
+    s5.j = torque_measured*1000;
+    s6.j = row_cmd;
+    s7.j = 1;
 
     T[2] = s1.C[0];
     T[3] = s1.C[1];
@@ -180,7 +191,7 @@
             uart.putc(T[i]);
             i++;
         }
-        if (i >= (sizeof(T)-1)) {
+        if (i >= sizeof(T)) {
             i = 0;
             break;
         }
@@ -204,6 +215,10 @@
 
 void find_torque()
 {
+
+//    Angle_Dif = Angle*3-D_Angle;
     angle_difference = (Angle*3-D_Angle)/4096.0f*2*PI;
+//    angle_difference = Angle_Dif/4096.0f*2*PI;
+
     torque_measured = angle_difference*ks;
-}
\ No newline at end of file
+}