Betago / Mbed 2 deprecated testmotoe

1

Dependencies:   PID mbed

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?

UserRevisionLine numberNew 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 }