Diff: main.cpp

Revision:

2:34775cb637e6

Parent:

1:fca866f76ca0

Child:

3:587916e9f3d0

diff -r fca866f76ca0 -r 34775cb637e6 main.cpp
--- a/main.cpp	Thu Dec 20 06:43:46 2018 +0000
+++ b/main.cpp	Thu Dec 20 07:20:06 2018 +0000
@@ -29,51 +29,48 @@
     
     m3.write(0, RG_SOFT_SRV,RG_DEFAULT, 1);
 
+   while(1){ 
+
         data[0] = 1-f0;
         data[1] = 1-f1;
         data[2] = 1-f2;
         data[3] = 1-f3;
         data[4] = 1-f4;  
-   
-   while(1){      
-    /*  for(int j = 0; j <= 4; j++){  
+        
+      for(int j = 0; j <= 4; j++){  
         for(int i = 0;i <= 4; i++){
            tmp[i] = Nin.getc();               
            }    
         force[j] = atof(tmp); 
        }
- */        
+         
         //duty_m = f0;  
         //pc.printf("Duty_m:%.3f\r\n",duty_m);     
         //pc.printf("Duty:%.3f\r\n",duty);       
         //led.write(duty);
         
   /*      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[1],force[3]-data[1],force[4]-data[1]);     */
+                 ,force[0],force[1],force[2],force[3],force[4]);  
                  
         pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n"
-                 ,data[0],data[1],data[2],data[3],data[4]); 
-                      
-              
- /*   
-      for(int i = 0;i <= 3; i++){
+                 ,force[0]-data[0],force[1]-data[1],force[2]-data[1],force[3]-data[1],force[4]-data[1]);                   
+ */
+    
+      for(int i = 1;i <= 4; i++){
         
             //a+=(int32_t)(30000*diff);
-            a[i]+=30000*(data[i]-force[i]);   
+            a[i]+=30000*(force[i]-data[i]);   
                      
             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+1, a[i]*2/36000);      
-           //if(i==3) pc.printf("\n\r"); 
+           pc.printf("No.%d, Stroke:%ld mm ", i, a[i]*2/36000);      
+           if(i==4) pc.printf("\n\r"); 
             
-        wait(0.001);
-        }  */
+        //wait(0.001);
+        }  
     }
 }