File content as of revision 2:34775cb637e6:

#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[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(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;  
        
      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]);                   
 */
    
      for(int i = 1;i <= 4; i++){
        
            //a+=(int32_t)(30000*diff);
            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, a[i]*2/36000);      
           if(i==4) pc.printf("\n\r"); 
            
        //wait(0.001);
        }  
    }
}