Revision 1:c6c07cb81c1a, committed 2018-12-21

Comitter:

OscarLiao

Date:

Fri Dec 21 06:18:28 2018 +0000

Parent:

0:80f74d98d66f

Commit message:

test

Changed in this revision

diff -r 80f74d98d66f -r c6c07cb81c1a main.cpp
--- a/main.cpp	Fri Nov 30 13:11:38 2018 +0000
+++ b/main.cpp	Fri Dec 21 06:18:28 2018 +0000
@@ -74,9 +74,9 @@
 #define L0          0.2f           //Length of the leg
 
 //Structure
-#define PWM_Hip     1410            //900~1550~2200 2050
-#define PWM_Thigh   1490            //900~1550~2200 1300
-#define PWM_Shin    1710            //900~1550~2200 1450 1740
+#define PWM_Hip     1460            //900~1550~2200 2050
+#define PWM_Thigh   1330            //900~1550~2200 1300
+#define PWM_Shin    1780            //900~1550~2200 1450 1740
 
 #define Buff_size   16              //Serial Buffer
 
@@ -214,8 +214,12 @@
     Kd_Hip*KN, Kd_Thigh*KN, Kd_Shin*KN                //{Hip, Thigh, Shin}
 };
 float q_0_E[3] = {                  //"Controller Input" from upper controller in endpoint level (Operational Space)
-    0, 0, 0                         //{Hip, leg length, leg angle} original point
+    0, 0.2, 0                         //{Hip, leg length, leg angle} original point
 };
+
+float q_0_E_1_gain = 0.2f/255.0f;
+float q_0_E_2_gain = (pi/4)/128.0f;
+
 float T_0[3] = {                    //"Controller Input" from upper controller in joint level
     0, 0, 0                         //{T_0_Hip, T_0_Tigh, F_0_Stretch} force feed foward
 };
@@ -286,15 +290,6 @@
 };
 
 //╔═════════════════╗
-//║   sine signal   ║
-//╚═════════════════╝
-int data = 0;
-float amp = 0.1;
-float freq = 55; //rad/s
-int totalticks = 0;
-int ticks = 0;
-
-//╔═════════════════╗
 //║   I/O Serial    ║
 //╚═════════════════╝
 uint8_t Buff[Buff_size];
@@ -337,18 +332,13 @@
     init_IO();                              //initialized value
     init_TIMER();                           //start TT_main
     
-    //for sine signal
-    totalticks = ((2*pi)/freq)/((Rms/1E6)*(Task_2_NN+1));
     Cmd.attach(&Rx_irq,Serial::RxIrq);      //start recieving message
 //    NVIC_DisableIRQ(USART1_IRQn);           //stay down before CS1 signal
 
     CS1.fall(&Rx_srt);                      //attach the address of Rx_srt to falling edge as start signal of communication
     CS1.rise(&Rx_end);                      //attach the address of Rx_end to rise edge
     
-    q_0_E[0] = 0;
-    q_0_E[1] = 0.2;
-    q_0_E[2] = 0.0;
-    
+    //for joint endpoint transform
     Phi1_0 = (acos((L0*L0+L1*L1-L2*L2)/(2*L0*L1)));
     Phi2_0 = (Phi1_0 + acos((L0*L0+L2*L2-L1*L1)/(2*L0*L2)));
             
@@ -386,8 +376,9 @@
 //            q_0[2] = -0.9;
 //            Debug.printf("%f\n", q_act_est[1]);
 //            Debug.printf("%f, %f, %f\r", q_0[0], q_0[1], q_0[2]);
-            Debug.printf("%lf\r", adc3.read());
+//            Debug.printf("%lf\r", adc3.read());
 //            Debug.printf("%f\n", 123.23);
+            Debug.printf("%d %f\r", Buff[2], q_0_E[2]);
 //            Debug.printf("%.f\r", q_ref[0]);L_act_est
 //            Debug.printf("%.2f\r", q_spring_0[0]);
 //            Debug.printf("%.2f\r", J33);
@@ -397,43 +388,6 @@
             Flag_1 = 0;                 //clear pending
             Task_1 = 0;                 //clear sign
         }
-#if DEBUG
-        if(Flag_2 == 1) {               //check pending
-            //Task_2 start
-            
-            //%Debug.printf("%d, %d, %d\r", PWM[0], PWM[1], PWM[2]);
-//            Debug.printf("%f\n", q_act_est[2]); 
-//            
-//            if(data)
-//            {
-//                //generate sine signal
-//                //q_0[2] = amp*sin((ticks/(float)totalticks)*(2*pi));
-////                ticks++;
-////                ticks = ticks%(totalticks+1);
-//                //Debug.printf("%f\n", q_act_est[2]);      
-//                q_0[2] = 0.5; 
-//            }
-//            else
-//            {
-//                q_0[2] = 0; 
-//            }
-            
-            //Task_2 done
-            
-            Flag_2 = 0;                 //clear pending
-            Task_2 = 0;
-        }
-#endif
-        //data test
-        if(Debug.readable()) {
-//            Debug.gets(buffer, 4);
-//            q_0[0] = ((pi/2)/256) * (buffer[0] - 238);
-//            q_0[1] = ((pi/2)/256) * (buffer[1] - 128);
-//            q_0[2] = ((pi/2)/256) * (buffer[2] - 128);  
-            q_0_E[1] -= 0.05; 
-            Debug.getc();
-            data = !data;    
-        }  
         
         if(Cmd_Flag == 1) {
             //{...}{Hip, Len, Phi}, {'#'}
@@ -441,10 +395,9 @@
             led = 1;
             if(Buff[3] == 'Q') {
                 //q_0 command
-                q_0_E[0] = (Buff[0]);
-                q_0_E[1] = 0.1f + 0.2f * (Buff[1]/255.0f);
-                q_0_E[2] = (Buff[2]);
-                //Debug.printf("%d %f\n", Buff[1], q_0_E[1]);
+                q_0_E[0] = (Buff[0] - 128);
+                q_0_E[1] = 0.1f + q_0_E_1_gain * Buff[1];
+                q_0_E[2] = q_0_E_2_gain * (Buff[2]-128);
             } else if(Buff[3] == 'T') {
                 //T_0 command
                 T_0[0] = 1.0f * (Buff[0] - 128);
@@ -460,7 +413,7 @@
                 B_c[2] = 0.02f * (Buff[2] - 128);
             } else {
             //Show error in communication
-//                led = 1;
+                led2 = 1;
             }
 
             Buff[3] = 0x00;           //clear end signals
@@ -626,7 +579,7 @@
             T_reg_I[i] =  constrain(T_reg_I[i], -200, 200);
             q_ref[i] = T_err[i]*Kp[i] + T_reg_I[i] + q_ref[i];
         }
-        q_ref[0] = q_0_E[0]*(550/(float)(0.5*pi));
+        q_ref[0] = (q_0_E[0]/128.0f)*225;
         q_ref[0] = constrain(q_ref[0], -650, 650);
         q_ref[1] = constrain(q_ref[1], -650, 650);
         q_ref[2] = constrain(q_ref[2], -650, 650);