File content as of revision 0:c52fc2fdd2e0:

#include "mbed.h"
#include "move.h"
#include "PID.h"
Serial PC(SERIAL_TX,SERIAL_RX);
move m1;
DigitalOut myled(LED1);
//encoder

InterruptIn encoderA_1(PC_10);//PB_1
DigitalIn encoderB_1(PC_12);//PB_2
InterruptIn encoderA_2(PA_5);//PB_14
DigitalIn encoderB_2(PA_6);//PB_15
int Encoderpos=0;
float valocity1 =0,valocity2 =0,pulse_1=0,pulse_2=0,count=0,r=0.125;
Timer timerStart;
//timer
 int timer_now=0,timer_later=0;
 int times=0,timer_buffer=0;
 //pid

double setp1=0,setp2=0;
float outPID =0;
float VRmax=0,VLmax=0,VR=0,VL=0,KP_LEFT=0,KI_LEFT=0,KD_LEFT=0,KP_RIGHT=0,KI_RIGHT=0 ,KD_RIGHT=0 ;
PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1);
PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1);
 void EncoderA_1()//ซ้าย
{   if(encoderB_1==0)
        { Encoderpos = Encoderpos + 1;}
    else
   { Encoderpos = Encoderpos -1;}
   pulse_1+=1;
  // PC.printf("m1=%d\n",Encoderpos);
   
}
  void EncoderA_2()//ขวา
{ 
    if(encoderB_2==0)
    { Encoderpos = Encoderpos + 1;}
    else
    { Encoderpos = Encoderpos -1;}
    pulse_2+=1;
   // PC.printf("m2=%d\n",Encoderpos);
}
void getvelo1()// encoder
{
    valocity1=pulse_1*((2*3.14*r)/128);
    PC.printf("valocity1=%f  \n",valocity1);
    timerStart.reset();
}
void getvelo2()
{
    valocity2=pulse_2*((2*3.14*r)/128);
    PC.printf("valocity2=%f  \n",valocity2);
    timerStart.reset();
}
double map(double x, double in_min, double in_max, double out_min, double out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
    
}
void PID_m1()//left
{
    setp1=map(1.0,0.0,1.094,0.0,1.0);
    P1.setSetPoint(setp1);
     times=timerStart.read();
       if(times==1)// m/s
       {   
           getvelo1();
           //pc.printf("TIME \n");
           times=0;
           pulse_1=0;
        }
    P1.setProcessValue(valocity1);
    outPID=P1.compute();
     //pc.printf("outPID=%f \n",outPID);
     m1.movespeed_1(setp1,outPID);
}
void PID_m2()//right
{
    setp2=map(1.0,0.0,1.094,0.0,1.0);
    P2.setSetPoint(setp2);
     times=timerStart.read();
       if(times==1)// m/s
       {   
           getvelo2();
           //pc.printf("TIME \n");
           times=0;
           pulse_2=0;
        }
    P2.setProcessValue(valocity2);
    outPID=P2.compute();
     //pc.printf("outPID=%f \n",outPID);
     m1.movespeed_2(setp2,outPID);
}
int main() {
    encoderA_1.rise(&EncoderA_1);
    encoderA_2.rise(&EncoderA_2);
    PC.baud(115200);
    
    
    while(1) {
        myled = !myled;
        m1.movespeed_1(1,0);
        m1.movespeed_2(1,0);
        times=timerStart.read();
       if(times==1)// m/s
       {   
           getvelo1();
           getvelo2();
           //pc.printf("TIME \n");
           times=0;
           pulse_1=0;
           pulse_2=0;
        }
        /*dirr1=0;
        dirr2=1;
        speeds2=1;
        wait(1);*/
    }
}