File content as of revision 12:8a06a803e373:

#include"mbed.h"
#include "move.h"
#include "PID.h"
#include "UNTRASONIC.h"

DigitalOut led1(LED1);
InterruptIn encoderA_d(PB_12);
DigitalIn encoderB_d(PB_13);
InterruptIn encoderA_1(PB_1);
DigitalIn encoderB_1(PB_2);
InterruptIn encoderA_2(PB_14);
DigitalIn encoderB_2(PB_15);
Timer timerStart;
Timeout time_getsensor;
Timeout time_distance;
Timeout shutdown;
move m1;
PID P1(0.005,0.005,0,0.1);
double setp1=0,setp2=0;
//DigitalIn encoderB(D5);
 
 int timer_now=0,timer_later=0;
 Serial pc(SERIAL_TX,SERIAL_RX);
 int Encoderpos = 0,times=0;
 int real_d=0;
 float valocity =0,pulse_1=0,pulse_2=0,count=0,r=0.125,velocityreal=0,pulse_d=0,Z_d=0;
 float outPID =0;
// bool timeout_state=0;
 //double Input,Output,setp,Kp=0.005,Ki=0.005,Kd=0;
 sensor s1;
void Rx_interrupt()
{
    //s1.get_motor();รับค่ามอเตอร์
    timer_later= timer_now;
  
}
void EncoderA_1()
{   if(encoderB_1==0)
        { Encoderpos = Encoderpos + 1;}
    else
   { Encoderpos = Encoderpos -1;}
   pulse_1+=1;
   //Encoderpos = Encoderpos + 1;
   //valocity+=1;
   //pc.printf("%d \n",Encoderpos);
   //pc.printf("pulse=%f  \n",pulse);
   //if(pulse==128)
   //{count+=1;pulse=0; pc.printf("count=%f  \n",count);}
}
  void EncoderA_2()
{ 
    if(encoderB_2==0)
    { Encoderpos = Encoderpos + 1;}
    else
    { Encoderpos = Encoderpos -1;}
    pulse_2+=1;
    //pc.printf("%d",Encoderpos);
}
void EncoderA_D()
{ 
    if(encoderB_d==0)
    { Encoderpos = Encoderpos + 1;}
    else
    { Encoderpos = Encoderpos -1;}
    pulse_d+=1;
    if(pulse_d==128)
    {
        Z_d+=1;
        pulse_d=0;
    }
   // pc.printf("%d",Encoderpos);
}
void getvelo1()//จาก encoder
{
    valocity=pulse_1*((2*3.14*r)/128);
    pc.printf("valocity=%f  \n",valocity);
    count=0;
    timerStart.reset();
}
void getvelo2()
{
    valocity=pulse_2*((2*3.14*r)/128);
    pc.printf("valocity=%f  \n",valocity);
    count=0;
    timerStart.reset();
}
void get_d()//ระยะทาง
{
    real_d=Z_d*(2*3.14*r);
    //ส่งข้อมูล
    
}
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()
{
    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(valocity);
    outPID=P1.compute();
     //pc.printf("outPID=%f \n",outPID);
     m1.movespeed_1(1,setp1,outPID);
}
void PID_m2()
{
    setp2=map(1.0,0.0,1.094,0.0,1.0);
    P1.setSetPoint(setp2);
     times=timerStart.read();
       if(times==1)// m/s
       {   
           getvelo2();
           //pc.printf("TIME \n");
           times=0;
           pulse_2=0;
        }
    P1.setProcessValue(valocity);
    outPID=P1.compute();
     //pc.printf("outPID=%f \n",outPID);
     m1.movespeed_2(1,setp2,outPID);
}
void getSensor()
{
    s1.get_sen();
}

int main()
{
      
       
       pc.baud(115200);
       encoderA_1.rise(&EncoderA_1);
       timerStart.start();   
       P1.setMode(1);
       P1.setBias(0);
     //  pc.printf("READY \n");
       pc.attach(&Rx_interrupt, Serial::RxIrq);
       led1=1;
    while(1)
    
    { 
     timer_now=timerStart.read();
     if((timer_now-timer_later)>2)
       {
            m1.movespeed_1(1,0,0);
            m1.movespeed_2(1,0,0);
       }
       //shutdown.attach_us(&Timeout, 2.0);
       else
       {time_getsensor.attach_us(&getSensor, 2.0);
       time_distance.attach_us(&get_d, 2.0);
       PID_m1();
       PID_m2();}
       }  
}