File content as of revision 4:5878229a7dfa:

#include "mbed.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);

int main()
{
    //float k=0.5;
    float data[5];
    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[4];
    //led.period_ms(10);
    
    Nin.baud(115200);
    
    m3.write(0, RG_SOFT_SRV,RG_DEFAULT, 1);
    m3.write(0, RG_CYCLE,RG_COMMUNICATE, 500000);
    m3.write(0, RG_CYCLE,RG_INTERPOLATE, 100);
    m3.write(0, RG_STATE,RG_DEFAULT, 11);

   while(1){ 

        data[0] = 1-f0;
        data[1] = 1-f1;
        data[2] = 1-f2;
        data[3] = 1-f3;
        data[4] = 1-f4;  
        
      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[2],force[3]-data[2],force[4]-data[2]);       
                
        pc.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n"
                 ,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);
            if(force[i]*k-data[i]>=0){
            a[i]+=90000*(force[i]*k-data[i]);   
            }else{
            a[i]-=810000*(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"); 
            
        //wait(0.001);
        } 
/*        for(int i = 1;i <= 4; i++){
            float k = 0.1;
            //a+=(int32_t)(30000*diff);
            if(force[i]*k-data[i]>=0){
            a[i]+=90000*(0.5f-data[i]);   
            }else{
            a[i]-=90000*(data[i]-0.5f);          
            }
                     
            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"); 
         } */  

    }
}