Revision 1:afe181fc0401, committed 2018-08-23

Comitter:

JJting

Date:

Thu Aug 23 06:44:19 2018 +0000

Parent:

0:5ab373bcfe6d

Commit message:

for SEA

Changed in this revision

diff -r 5ab373bcfe6d -r afe181fc0401 Torque_Control_Current.cpp
--- a/Torque_Control_Current.cpp	Mon Jul 23 08:36:38 2018 +0000
+++ b/Torque_Control_Current.cpp	Thu Aug 23 06:44:19 2018 +0000
@@ -17,11 +17,12 @@
 float Ts = ITR_time/1000000;
 
 // PID
-PID motor_pid(7.2, 0, 0.13, Ts);// 6.4 0.13   7.2 0.13
+PID motor_pid(7.2, 0, 0, Ts);// 6.4 0.13   7.2 0.13
 float PIDout = 0.0f;
 // Pin
 Serial pc(USBTX, USBRX);
 SPI spi(D4, D5, D3); // mosi, miso, sclk
+//SPI spi(D11, D12, D13); // mosi, miso, sclk
 DigitalOut encoder_cs(D9);
 DigitalOut LED(A4);
 AnalogIn Value(A5);
@@ -30,11 +31,12 @@
 unsigned short encoder_value = 0;
 unsigned short angle = 0;
 unsigned short angle_old = 0;
-unsigned short angle_init ;
+unsigned short angle_init;
 
 // Dynamixel
 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
 int servo_cmd;
+int row_cmd;
 int theta_l;
 bool servo_dir;
 int omega;
@@ -73,7 +75,7 @@
     char C[2];
     // C[0] is lowbyte of j, C[1] is highbyte of j
 };
-char T[10] = {255,255,0,0,0,0,0,0,0,0};
+char T[16] = {255,255,0,0,0,0,0,0,0,0,0,0,0,0};
 
 int i = 0;
 int j = 0;
@@ -106,8 +108,8 @@
     wait_ms(500);
     LED = 0;
     init_TIMER();
-    
-    
+
+
     while(1) {
     }
 }
@@ -119,7 +121,9 @@
 
 void init_TIMER()
 {
+    LED = 1;
     timer1.attach_us(&timer1_ITR, ITR_time); // the address of the function to be attached (timer1_ITR) and the interval (1000 micro-seconds)
+    LED = 0;
 }
 
 void init_SPI()
@@ -135,9 +139,14 @@
 void init_position()
 {
 
-    angle_init = 2038;
-    
-    wait_ms(1000);    
+//    angle_init = 2038;
+    encoder_cs = 0;  // Select the device by seting chip select low
+
+    encoder_value = spi.write(0x00);
+    angle_init = encoder_value >> 3;
+
+    encoder_cs = 1;  // Deselect the device
+    wait_ms(1000);
 }
 void init_DYNAMIXEL()
 {
@@ -146,7 +155,7 @@
 }
 
 void timer1_ITR()
-{   
+{
 
     angle_measured();
     find_torque();
@@ -167,29 +176,35 @@
         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, 0);
 //    wait_ms(1);
 //    speed = dynamixelClass.readSpeed(SERVO_ID);
     UART_TX();
 }
 
 void angle_measured()
-{   
+{
     Angle_old = Angle;
-    
+
     encoder_cs = 0;  // Select the device by seting chip select low
-    
+
     encoder_value = spi.write(0x00);
     angle = encoder_value >> 3;
-    
+
     encoder_cs = 1;  // Deselect the device
     D_angle = dynamixelClass.readPosition(SERVO_ID);
-    
+
     angle_dif = relative_angle(angle,angle_init);
-    D_angle_dif = relative_angle(D_angle,D_angle_init);;
-    
+    D_angle_dif = relative_angle(D_angle,D_angle_init);
+
     Angle = -(D_angle_dif)+angle_dif;
-    
+
 }
 
 int relative_angle(unsigned int pos,unsigned int init_pos)
@@ -202,12 +217,12 @@
         angle_dif = -(-4096-(angle_dif));
     else
         angle_dif = angle_dif;
-    
+
     return angle_dif;
 }
 
 void find_torque()
-{                
+{
     angle_difference = angle_dif/4096.0f*2*PI;
     torque_measured = angle_difference*ks;
 }
@@ -220,13 +235,19 @@
     splitter splitter2;
     splitter splitter3;
     splitter splitter4;
+    splitter splitter5;
+    splitter splitter6;
+    splitter splitter7;
 
     //數值範圍：-32768~32768
-    splitter1.j = torque_ref*1000;
+    splitter1.j = row_cmd*100;
     splitter2.j = torque_measured*1000;
-    splitter3.j = servo_cmd;
-    splitter4.j = 0;
-
+//    splitter2.j = D_angle;
+    splitter3.j = D_angle;
+    splitter4.j = angle;
+    splitter5.j = angle_init;
+    splitter6.j = angle_dif;
+    splitter7.j = 1;
 
     T[2] = splitter1.C[0];
     T[3] = splitter1.C[1];
@@ -236,13 +257,18 @@
     T[7] = splitter3.C[1];
     T[8] = splitter4.C[0];
     T[9] = splitter4.C[1];
-
+    T[10] = splitter5.C[0];
+    T[11] = splitter5.C[1];
+    T[12] = splitter6.C[0];
+    T[13] = splitter6.C[1];
+    T[14] = splitter7.C[0];
+    T[15] = splitter7.C[1];
     while (1) {
         if (pc.writeable() == 1) {
             pc.putc(T[i]);
             i++;
         }
-        if (i>9) {
+        if (i>15) {
             i = 0;
             break;
         }