File content as of revision 7:2daffd310c71:

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

DigitalOut led1(LED1);
InterruptIn encoderA(D10);
InterruptIn encoderB(D11);
Timer timerStart;
Timeout timecount;
move m1;
PID P1(0.005,0.005,0,0.1);
double setp=0;
//DigitalIn encoderB(D5);


Serial pc(SERIAL_TX,SERIAL_RX);
 int Encoderpos = 0;
 float valocity =0,pulse=0,count=0,r=0.125,velocityreal=0;
 float outPID =0;
 //double Input,Output,setp,Kp=0.005,Ki=0.005,Kd=0;


void EncoderA()
{   if(encoderB==0)
        { Encoderpos = Encoderpos + 1;}
    else
   { Encoderpos = Encoderpos -1;}
   
   //Encoderpos = Encoderpos + 1;
  // valocity+=1;
  // pc.printf("%d \n",Encoderpos);
  pulse+=1;
  //pc.printf("pulse=%f  \n",pulse);
 // if(pulse==128)
  //{count+=1;pulse=0; pc.printf("count=%f  \n",count);}
  
}
  

/*void EncoderB()
{ 
    if(encoderA==1)
    { Encoderpos = Encoderpos + 1;}
    else
    { Encoderpos = Encoderpos -1;}
    pc.printf("%d",Encoderpos);
}*/
void getvelo()
{
    valocity=pulse*((2*3.14*r)/128);
    pc.printf("valocity=%f  \n",valocity);
    count=0;
    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;
    
}

int main()
{
       int times=0;
       
       pc.baud(115200);
       encoderA.rise(&EncoderA);
       timerStart.start();   
       P1.setMode(1);
       //setp=map(1,0,1.0916,0,1);
       setp=map(1.0,0.0,1.094,0.0,1.0);
       
      // P1.setSetPoint(velocityreal);//setpont
        P1.setSetPoint(setp);//setpont
       P1.setBias(0);
       pc.printf("READY \n");
      
       led1=1;
    while(1)
    
    {  
       // pc.printf("%f  \n",setp);
       times=timerStart.read();
       if(times==1)// m/s
       {   
           getvelo();
           //pc.printf("TIME \n");
           times=0;
           pulse=0;
        }
      
       P1.setProcessValue(valocity);
       // m1.movespeed(1,1.0);
        outPID=P1.compute();
         pc.printf("outPID=%f \n",outPID);
        m1.movespeed(1,setp,outPID);
       // m1.movespeed(1,1,0);
        
        wait(0.1);
    
       
         
    }  
}