![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
1
main.cpp
- Committer:
- palmdotax
- Date:
- 2016-06-27
- Revision:
- 0:c52fc2fdd2e0
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);*/ } }