Betago
/
testmotoe
1
main.cpp@0:c52fc2fdd2e0, 2016-06-27 (annotated)
- Committer:
- palmdotax
- Date:
- Mon Jun 27 21:14:19 2016 +0000
- Revision:
- 0:c52fc2fdd2e0
1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
palmdotax | 0:c52fc2fdd2e0 | 1 | #include "mbed.h" |
palmdotax | 0:c52fc2fdd2e0 | 2 | #include "move.h" |
palmdotax | 0:c52fc2fdd2e0 | 3 | #include "PID.h" |
palmdotax | 0:c52fc2fdd2e0 | 4 | Serial PC(SERIAL_TX,SERIAL_RX); |
palmdotax | 0:c52fc2fdd2e0 | 5 | move m1; |
palmdotax | 0:c52fc2fdd2e0 | 6 | DigitalOut myled(LED1); |
palmdotax | 0:c52fc2fdd2e0 | 7 | //encoder |
palmdotax | 0:c52fc2fdd2e0 | 8 | |
palmdotax | 0:c52fc2fdd2e0 | 9 | InterruptIn encoderA_1(PC_10);//PB_1 |
palmdotax | 0:c52fc2fdd2e0 | 10 | DigitalIn encoderB_1(PC_12);//PB_2 |
palmdotax | 0:c52fc2fdd2e0 | 11 | InterruptIn encoderA_2(PA_5);//PB_14 |
palmdotax | 0:c52fc2fdd2e0 | 12 | DigitalIn encoderB_2(PA_6);//PB_15 |
palmdotax | 0:c52fc2fdd2e0 | 13 | int Encoderpos=0; |
palmdotax | 0:c52fc2fdd2e0 | 14 | float valocity1 =0,valocity2 =0,pulse_1=0,pulse_2=0,count=0,r=0.125; |
palmdotax | 0:c52fc2fdd2e0 | 15 | Timer timerStart; |
palmdotax | 0:c52fc2fdd2e0 | 16 | //timer |
palmdotax | 0:c52fc2fdd2e0 | 17 | int timer_now=0,timer_later=0; |
palmdotax | 0:c52fc2fdd2e0 | 18 | int times=0,timer_buffer=0; |
palmdotax | 0:c52fc2fdd2e0 | 19 | //pid |
palmdotax | 0:c52fc2fdd2e0 | 20 | |
palmdotax | 0:c52fc2fdd2e0 | 21 | double setp1=0,setp2=0; |
palmdotax | 0:c52fc2fdd2e0 | 22 | float outPID =0; |
palmdotax | 0:c52fc2fdd2e0 | 23 | 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 ; |
palmdotax | 0:c52fc2fdd2e0 | 24 | PID P1(KP_LEFT,KI_LEFT,KD_LEFT,0.1); |
palmdotax | 0:c52fc2fdd2e0 | 25 | PID P2(KP_RIGHT,KI_RIGHT ,KD_RIGHT,0.1); |
palmdotax | 0:c52fc2fdd2e0 | 26 | void EncoderA_1()//ซ้าย |
palmdotax | 0:c52fc2fdd2e0 | 27 | { if(encoderB_1==0) |
palmdotax | 0:c52fc2fdd2e0 | 28 | { Encoderpos = Encoderpos + 1;} |
palmdotax | 0:c52fc2fdd2e0 | 29 | else |
palmdotax | 0:c52fc2fdd2e0 | 30 | { Encoderpos = Encoderpos -1;} |
palmdotax | 0:c52fc2fdd2e0 | 31 | pulse_1+=1; |
palmdotax | 0:c52fc2fdd2e0 | 32 | // PC.printf("m1=%d\n",Encoderpos); |
palmdotax | 0:c52fc2fdd2e0 | 33 | |
palmdotax | 0:c52fc2fdd2e0 | 34 | } |
palmdotax | 0:c52fc2fdd2e0 | 35 | void EncoderA_2()//ขวา |
palmdotax | 0:c52fc2fdd2e0 | 36 | { |
palmdotax | 0:c52fc2fdd2e0 | 37 | if(encoderB_2==0) |
palmdotax | 0:c52fc2fdd2e0 | 38 | { Encoderpos = Encoderpos + 1;} |
palmdotax | 0:c52fc2fdd2e0 | 39 | else |
palmdotax | 0:c52fc2fdd2e0 | 40 | { Encoderpos = Encoderpos -1;} |
palmdotax | 0:c52fc2fdd2e0 | 41 | pulse_2+=1; |
palmdotax | 0:c52fc2fdd2e0 | 42 | // PC.printf("m2=%d\n",Encoderpos); |
palmdotax | 0:c52fc2fdd2e0 | 43 | } |
palmdotax | 0:c52fc2fdd2e0 | 44 | void getvelo1()// encoder |
palmdotax | 0:c52fc2fdd2e0 | 45 | { |
palmdotax | 0:c52fc2fdd2e0 | 46 | valocity1=pulse_1*((2*3.14*r)/128); |
palmdotax | 0:c52fc2fdd2e0 | 47 | PC.printf("valocity1=%f \n",valocity1); |
palmdotax | 0:c52fc2fdd2e0 | 48 | timerStart.reset(); |
palmdotax | 0:c52fc2fdd2e0 | 49 | } |
palmdotax | 0:c52fc2fdd2e0 | 50 | void getvelo2() |
palmdotax | 0:c52fc2fdd2e0 | 51 | { |
palmdotax | 0:c52fc2fdd2e0 | 52 | valocity2=pulse_2*((2*3.14*r)/128); |
palmdotax | 0:c52fc2fdd2e0 | 53 | PC.printf("valocity2=%f \n",valocity2); |
palmdotax | 0:c52fc2fdd2e0 | 54 | timerStart.reset(); |
palmdotax | 0:c52fc2fdd2e0 | 55 | } |
palmdotax | 0:c52fc2fdd2e0 | 56 | double map(double x, double in_min, double in_max, double out_min, double out_max) |
palmdotax | 0:c52fc2fdd2e0 | 57 | { |
palmdotax | 0:c52fc2fdd2e0 | 58 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
palmdotax | 0:c52fc2fdd2e0 | 59 | |
palmdotax | 0:c52fc2fdd2e0 | 60 | } |
palmdotax | 0:c52fc2fdd2e0 | 61 | void PID_m1()//left |
palmdotax | 0:c52fc2fdd2e0 | 62 | { |
palmdotax | 0:c52fc2fdd2e0 | 63 | setp1=map(1.0,0.0,1.094,0.0,1.0); |
palmdotax | 0:c52fc2fdd2e0 | 64 | P1.setSetPoint(setp1); |
palmdotax | 0:c52fc2fdd2e0 | 65 | times=timerStart.read(); |
palmdotax | 0:c52fc2fdd2e0 | 66 | if(times==1)// m/s |
palmdotax | 0:c52fc2fdd2e0 | 67 | { |
palmdotax | 0:c52fc2fdd2e0 | 68 | getvelo1(); |
palmdotax | 0:c52fc2fdd2e0 | 69 | //pc.printf("TIME \n"); |
palmdotax | 0:c52fc2fdd2e0 | 70 | times=0; |
palmdotax | 0:c52fc2fdd2e0 | 71 | pulse_1=0; |
palmdotax | 0:c52fc2fdd2e0 | 72 | } |
palmdotax | 0:c52fc2fdd2e0 | 73 | P1.setProcessValue(valocity1); |
palmdotax | 0:c52fc2fdd2e0 | 74 | outPID=P1.compute(); |
palmdotax | 0:c52fc2fdd2e0 | 75 | //pc.printf("outPID=%f \n",outPID); |
palmdotax | 0:c52fc2fdd2e0 | 76 | m1.movespeed_1(setp1,outPID); |
palmdotax | 0:c52fc2fdd2e0 | 77 | } |
palmdotax | 0:c52fc2fdd2e0 | 78 | void PID_m2()//right |
palmdotax | 0:c52fc2fdd2e0 | 79 | { |
palmdotax | 0:c52fc2fdd2e0 | 80 | setp2=map(1.0,0.0,1.094,0.0,1.0); |
palmdotax | 0:c52fc2fdd2e0 | 81 | P2.setSetPoint(setp2); |
palmdotax | 0:c52fc2fdd2e0 | 82 | times=timerStart.read(); |
palmdotax | 0:c52fc2fdd2e0 | 83 | if(times==1)// m/s |
palmdotax | 0:c52fc2fdd2e0 | 84 | { |
palmdotax | 0:c52fc2fdd2e0 | 85 | getvelo2(); |
palmdotax | 0:c52fc2fdd2e0 | 86 | //pc.printf("TIME \n"); |
palmdotax | 0:c52fc2fdd2e0 | 87 | times=0; |
palmdotax | 0:c52fc2fdd2e0 | 88 | pulse_2=0; |
palmdotax | 0:c52fc2fdd2e0 | 89 | } |
palmdotax | 0:c52fc2fdd2e0 | 90 | P2.setProcessValue(valocity2); |
palmdotax | 0:c52fc2fdd2e0 | 91 | outPID=P2.compute(); |
palmdotax | 0:c52fc2fdd2e0 | 92 | //pc.printf("outPID=%f \n",outPID); |
palmdotax | 0:c52fc2fdd2e0 | 93 | m1.movespeed_2(setp2,outPID); |
palmdotax | 0:c52fc2fdd2e0 | 94 | } |
palmdotax | 0:c52fc2fdd2e0 | 95 | int main() { |
palmdotax | 0:c52fc2fdd2e0 | 96 | encoderA_1.rise(&EncoderA_1); |
palmdotax | 0:c52fc2fdd2e0 | 97 | encoderA_2.rise(&EncoderA_2); |
palmdotax | 0:c52fc2fdd2e0 | 98 | PC.baud(115200); |
palmdotax | 0:c52fc2fdd2e0 | 99 | |
palmdotax | 0:c52fc2fdd2e0 | 100 | |
palmdotax | 0:c52fc2fdd2e0 | 101 | while(1) { |
palmdotax | 0:c52fc2fdd2e0 | 102 | myled = !myled; |
palmdotax | 0:c52fc2fdd2e0 | 103 | m1.movespeed_1(1,0); |
palmdotax | 0:c52fc2fdd2e0 | 104 | m1.movespeed_2(1,0); |
palmdotax | 0:c52fc2fdd2e0 | 105 | times=timerStart.read(); |
palmdotax | 0:c52fc2fdd2e0 | 106 | if(times==1)// m/s |
palmdotax | 0:c52fc2fdd2e0 | 107 | { |
palmdotax | 0:c52fc2fdd2e0 | 108 | getvelo1(); |
palmdotax | 0:c52fc2fdd2e0 | 109 | getvelo2(); |
palmdotax | 0:c52fc2fdd2e0 | 110 | //pc.printf("TIME \n"); |
palmdotax | 0:c52fc2fdd2e0 | 111 | times=0; |
palmdotax | 0:c52fc2fdd2e0 | 112 | pulse_1=0; |
palmdotax | 0:c52fc2fdd2e0 | 113 | pulse_2=0; |
palmdotax | 0:c52fc2fdd2e0 | 114 | } |
palmdotax | 0:c52fc2fdd2e0 | 115 | /*dirr1=0; |
palmdotax | 0:c52fc2fdd2e0 | 116 | dirr2=1; |
palmdotax | 0:c52fc2fdd2e0 | 117 | speeds2=1; |
palmdotax | 0:c52fc2fdd2e0 | 118 | wait(1);*/ |
palmdotax | 0:c52fc2fdd2e0 | 119 | } |
palmdotax | 0:c52fc2fdd2e0 | 120 | } |