Diff: main.cpp

Revision:

3:587916e9f3d0

Parent:

2:34775cb637e6

Child:

4:5878229a7dfa

--- a/main.cpp	Thu Dec 20 07:20:06 2018 +0000
+++ b/main.cpp	Tue Dec 25 08:24:47 2018 +0000
@@ -18,13 +18,13 @@
 {
     //float k=0.5;
     float data[5];
-    int32_t a[4]= {0};
+    int32_t a[5]= {0};
     //uint8_t error;   
     //Nex.baud(9600);
     //Nex.format(8,Serial::None,2);
     float force[5];
     //float force_r[5];
-    char tmp[5];
+    char tmp[4];
     //led.period_ms(10);
     
     m3.write(0, RG_SOFT_SRV,RG_DEFAULT, 1);
@@ -51,21 +51,30 @@
         
   /*      pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n"
                  ,force[0],force[1],force[2],force[3],force[4]);  
-                 
+          pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n"
+                 ,force[0]-data[0],force[1]-data[1],force[2]-data[2],force[3]-data[2],force[4]-data[2]);       
+                
         pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n"
-                 ,force[0]-data[0],force[1]-data[1],force[2]-data[1],force[3]-data[1],force[4]-data[1]);                   
- */
+                 ,data[0],data[1],data[2],data[3],data[4]);                   
+ */ 
+ 
+      // pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n"
+            //    ,force[0]-data[1],force[1]-data[2],force[2]-data[3],force[3]-data[4]); 
     
       for(int i = 1;i <= 4; i++){
-        
+            float k = 0.1;
             //a+=(int32_t)(30000*diff);
-            a[i]+=30000*(force[i]-data[i]);   
+            if(force[i]*k-data[i]>=0){
+            a[i]+=3000*(force[i]*k-data[i]);   
+            }else{
+            a[i]-=12000*(data[i]-force[i]*k);          
+            }
                      
             if(a[i]>300000)a[i]=300000;              
             else if(a[i]<0)a[i]=0; 
                         
             m3.write(i, RG_REFERENCE,RG_POSITION,a[i]*(-1));
-            
+                     
            pc.printf("No.%d, Stroke:%ld mm ", i, a[i]*2/36000);      
            if(i==4) pc.printf("\n\r");