Betago
/
testmotoe
1
Diff: main.cpp
- Revision:
- 0:c52fc2fdd2e0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jun 27 21:14:19 2016 +0000 @@ -0,0 +1,120 @@ +#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);*/ + } +}