Diff: main.cpp

Revision:

1:fca866f76ca0

Parent:

0:4134d186d76d

Child:

2:34775cb637e6

--- a/main.cpp	Thu Dec 13 13:38:35 2018 +0000
+++ b/main.cpp	Thu Dec 20 06:43:46 2018 +0000
@@ -1,72 +1,80 @@
 #include "mbed.h"
-#include "Usart.h"
+#include "M3_CAN.h"
 
 //PwmOut led(LED1);
 
+M3_CAN_mbed m3(D4, D10, 1000000);
+
 Serial Nin(PC_10,PC_11);
 Serial pc(SERIAL_TX, SERIAL_RX);
 
-/*
 AnalogIn f0(PA_0);
 AnalogIn f1(PA_1);
 AnalogIn f2(PA_4);
 AnalogIn f3(PB_0);
 AnalogIn f4(PC_1);
-*/
-
-///M3_USART m3(PA_9,PA_10,115200);
-//Serial pc(USBTX,USBRX);
-//DigitalOut myled(LED1);
-///DigitalIn myb(USER_BUTTON);
 
 int main()
 {
     //float k=0.5;
-    //int32_t a=0;
+    float data[5];
+    int32_t a[4]= {0};
     //uint8_t error;   
     //Nex.baud(9600);
     //Nex.format(8,Serial::None,2);
     float force[5];
+    //float force_r[5];
     char tmp[5];
     //led.period_ms(10);
     
-    //m3.write(RG_SOFT_SRV,RG_DEFAULT, 0);
-    //wait(1);  
-///    m3.write(RG_SOFT_SRV,RG_DEFAULT, 1);   
-///    m3.write(RG_PGAIN,RG_POSITION,2500);
-///    m3.write(RG_IGAIN,RG_POSITION,0);
-///    m3.write(RG_DGAIN,RG_POSITION,1000);
-    
+    m3.write(0, RG_SOFT_SRV,RG_DEFAULT, 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++){  
+   while(1){      
+    /*  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],force[1],force[2],force[3],force[4]);
-        
-        //diff = duty_m-duty;
+                 ,force[0]-data[0],force[1]-data[1],force[2]-data[1],force[3]-data[1],force[4]-data[1]);     */
+                 
+        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++){
         
-/*        a+=(int32_t)(30000*diff);
-
-
-            if(a>300000)a=300000;              
-            else if(a<0)a=0;   
+            //a+=(int32_t)(30000*diff);
+            a[i]+=30000*(data[i]-force[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));
             
-            m3.write(RG_REFERENCE,RG_POSITION,a*(-1));
-            pc.printf("Stroke:%ld mm\n\r",a*2/36000);*/
-
-              
-        //wait(0.001);
+           // pc.printf("No.%d, Stroke:%ld mm", i+1, a[i]*2/36000);      
+           //if(i==3) pc.printf("\n\r"); 
+            
+        wait(0.001);
+        }  */
     }
 }
 
+