Betago / Mbed 2 deprecated BETAGO

10/25/2015

Dependencies:   PID mbed MaxSonar eeprom iSerial Fork_Boss_Communication_Robot

main.cpp@12:8a06a803e373, 2016-03-03 (annotated)

Committer:
palmdotax
Date:
Thu Mar 03 09:07:22 2016 +0000
Revision:
12:8a06a803e373
Parent:
11:9df7ada37d31 
LOL

Who changed what in which revision?

UserRevisionLine numberNew contents of line
palmdotax 0:f02641e3fb91 1 #include"mbed.h"
palmdotax 1:7c3dbf140bfc 2 #include "move.h"
palmdotax 0:f02641e3fb91 3 #include "PID.h"
palmdotax 11:9df7ada37d31 4 #include "UNTRASONIC.h"
palmdotax 1:7c3dbf140bfc 5
palmdotax 3:365615fa646e 6 DigitalOut led1(LED1);
palmdotax 9:86701fec3f79 7 InterruptIn encoderA_d(PB_12);
palmdotax 9:86701fec3f79 8 DigitalIn encoderB_d(PB_13);
palmdotax 9:86701fec3f79 9 InterruptIn encoderA_1(PB_1);
palmdotax 9:86701fec3f79 10 DigitalIn encoderB_1(PB_2);
palmdotax 9:86701fec3f79 11 InterruptIn encoderA_2(PB_14);
palmdotax 9:86701fec3f79 12 DigitalIn encoderB_2(PB_15);
palmdotax 2:933d3edf38da 13 Timer timerStart;
palmdotax 11:9df7ada37d31 14 Timeout time_getsensor;
palmdotax 11:9df7ada37d31 15 Timeout time_distance;
palmdotax 11:9df7ada37d31 16 Timeout shutdown;
palmdotax 1:7c3dbf140bfc 17 move m1;
palmdotax 4:e6ab360e7de6 18 PID P1(0.005,0.005,0,0.1);
palmdotax 8:8fbc0c858875 19 double setp1=0,setp2=0;
palmdotax 0:f02641e3fb91 20 //DigitalIn encoderB(D5);
palmdotax 11:9df7ada37d31 21
palmdotax 11:9df7ada37d31 22 int timer_now=0,timer_later=0;
palmdotax 9:86701fec3f79 23 Serial pc(SERIAL_TX,SERIAL_RX);
palmdotax 9:86701fec3f79 24 int Encoderpos = 0,times=0;
palmdotax 9:86701fec3f79 25 int real_d=0;
palmdotax 9:86701fec3f79 26 float valocity =0,pulse_1=0,pulse_2=0,count=0,r=0.125,velocityreal=0,pulse_d=0,Z_d=0;
palmdotax 3:365615fa646e 27 float outPID =0;
palmdotax 11:9df7ada37d31 28 // bool timeout_state=0;
palmdotax 3:365615fa646e 29 //double Input,Output,setp,Kp=0.005,Ki=0.005,Kd=0;
palmdotax 11:9df7ada37d31 30 sensor s1;
palmdotax 11:9df7ada37d31 31 void Rx_interrupt()
palmdotax 11:9df7ada37d31 32 {
palmdotax 11:9df7ada37d31 33 //s1.get_motor();รับค่ามอเตอร์
palmdotax 11:9df7ada37d31 34 timer_later= timer_now;
palmdotax 11:9df7ada37d31 35
palmdotax 11:9df7ada37d31 36 }
palmdotax 9:86701fec3f79 37 void EncoderA_1()
palmdotax 9:86701fec3f79 38 { if(encoderB_1==0)
palmdotax 0:f02641e3fb91 39 { Encoderpos = Encoderpos + 1;}
palmdotax 0:f02641e3fb91 40 else
palmdotax 0:f02641e3fb91 41 { Encoderpos = Encoderpos -1;}
palmdotax 9:86701fec3f79 42 pulse_1+=1;
palmdotax 0:f02641e3fb91 43 //Encoderpos = Encoderpos + 1;
palmdotax 11:9df7ada37d31 44 //valocity+=1;
palmdotax 11:9df7ada37d31 45 //pc.printf("%d \n",Encoderpos);
palmdotax 9:86701fec3f79 46 //pc.printf("pulse=%f \n",pulse);
palmdotax 11:9df7ada37d31 47 //if(pulse==128)
palmdotax 11:9df7ada37d31 48 //{count+=1;pulse=0; pc.printf("count=%f \n",count);}
palmdotax 0:f02641e3fb91 49 }
palmdotax 9:86701fec3f79 50 void EncoderA_2()
palmdotax 0:f02641e3fb91 51 {
palmdotax 9:86701fec3f79 52 if(encoderB_2==0)
palmdotax 9:86701fec3f79 53 { Encoderpos = Encoderpos + 1;}
palmdotax 9:86701fec3f79 54 else
palmdotax 9:86701fec3f79 55 { Encoderpos = Encoderpos -1;}
palmdotax 9:86701fec3f79 56 pulse_2+=1;
palmdotax 9:86701fec3f79 57 //pc.printf("%d",Encoderpos);
palmdotax 9:86701fec3f79 58 }
palmdotax 9:86701fec3f79 59 void EncoderA_D()
palmdotax 9:86701fec3f79 60 {
palmdotax 9:86701fec3f79 61 if(encoderB_d==0)
palmdotax 0:f02641e3fb91 62 { Encoderpos = Encoderpos + 1;}
palmdotax 0:f02641e3fb91 63 else
palmdotax 0:f02641e3fb91 64 { Encoderpos = Encoderpos -1;}
palmdotax 9:86701fec3f79 65 pulse_d+=1;
palmdotax 9:86701fec3f79 66 if(pulse_d==128)
palmdotax 9:86701fec3f79 67 {
palmdotax 9:86701fec3f79 68 Z_d+=1;
palmdotax 9:86701fec3f79 69 pulse_d=0;
palmdotax 9:86701fec3f79 70 }
palmdotax 9:86701fec3f79 71 // pc.printf("%d",Encoderpos);
palmdotax 9:86701fec3f79 72 }
palmdotax 11:9df7ada37d31 73 void getvelo1()//จาก encoder
palmdotax 3:365615fa646e 74 {
palmdotax 9:86701fec3f79 75 valocity=pulse_1*((2*3.14*r)/128);
palmdotax 7:2daffd310c71 76 pc.printf("valocity=%f \n",valocity);
palmdotax 3:365615fa646e 77 count=0;
palmdotax 4:e6ab360e7de6 78 timerStart.reset();
palmdotax 0:f02641e3fb91 79 }
palmdotax 9:86701fec3f79 80 void getvelo2()
palmdotax 9:86701fec3f79 81 {
palmdotax 9:86701fec3f79 82 valocity=pulse_2*((2*3.14*r)/128);
palmdotax 9:86701fec3f79 83 pc.printf("valocity=%f \n",valocity);
palmdotax 9:86701fec3f79 84 count=0;
palmdotax 9:86701fec3f79 85 timerStart.reset();
palmdotax 9:86701fec3f79 86 }
palmdotax 11:9df7ada37d31 87 void get_d()//ระยะทาง
palmdotax 9:86701fec3f79 88 {
palmdotax 9:86701fec3f79 89 real_d=Z_d*(2*3.14*r);
palmdotax 11:9df7ada37d31 90 //ส่งข้อมูล
palmdotax 9:86701fec3f79 91
palmdotax 9:86701fec3f79 92 }
palmdotax 7:2daffd310c71 93 double map(double x, double in_min, double in_max, double out_min, double out_max)
palmdotax 5:68b740d113e6 94 {
palmdotax 6:9ed82a812ece 95 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
palmdotax 5:68b740d113e6 96
palmdotax 5:68b740d113e6 97 }
palmdotax 8:8fbc0c858875 98 void PID_m1()
palmdotax 8:8fbc0c858875 99 {
palmdotax 8:8fbc0c858875 100 setp1=map(1.0,0.0,1.094,0.0,1.0);
palmdotax 8:8fbc0c858875 101 P1.setSetPoint(setp1);
palmdotax 8:8fbc0c858875 102 times=timerStart.read();
palmdotax 8:8fbc0c858875 103 if(times==1)// m/s
palmdotax 8:8fbc0c858875 104 {
palmdotax 9:86701fec3f79 105 getvelo1();
palmdotax 8:8fbc0c858875 106 //pc.printf("TIME \n");
palmdotax 8:8fbc0c858875 107 times=0;
palmdotax 9:86701fec3f79 108 pulse_1=0;
palmdotax 8:8fbc0c858875 109 }
palmdotax 8:8fbc0c858875 110 P1.setProcessValue(valocity);
palmdotax 8:8fbc0c858875 111 outPID=P1.compute();
palmdotax 8:8fbc0c858875 112 //pc.printf("outPID=%f \n",outPID);
palmdotax 8:8fbc0c858875 113 m1.movespeed_1(1,setp1,outPID);
palmdotax 8:8fbc0c858875 114 }
palmdotax 8:8fbc0c858875 115 void PID_m2()
palmdotax 8:8fbc0c858875 116 {
palmdotax 8:8fbc0c858875 117 setp2=map(1.0,0.0,1.094,0.0,1.0);
palmdotax 8:8fbc0c858875 118 P1.setSetPoint(setp2);
palmdotax 8:8fbc0c858875 119 times=timerStart.read();
palmdotax 8:8fbc0c858875 120 if(times==1)// m/s
palmdotax 8:8fbc0c858875 121 {
palmdotax 9:86701fec3f79 122 getvelo2();
palmdotax 8:8fbc0c858875 123 //pc.printf("TIME \n");
palmdotax 8:8fbc0c858875 124 times=0;
palmdotax 9:86701fec3f79 125 pulse_2=0;
palmdotax 8:8fbc0c858875 126 }
palmdotax 8:8fbc0c858875 127 P1.setProcessValue(valocity);
palmdotax 8:8fbc0c858875 128 outPID=P1.compute();
palmdotax 8:8fbc0c858875 129 //pc.printf("outPID=%f \n",outPID);
palmdotax 8:8fbc0c858875 130 m1.movespeed_2(1,setp2,outPID);
palmdotax 8:8fbc0c858875 131 }
palmdotax 11:9df7ada37d31 132 void getSensor()
palmdotax 11:9df7ada37d31 133 {
palmdotax 11:9df7ada37d31 134 s1.get_sen();
palmdotax 11:9df7ada37d31 135 }
palmdotax 0:f02641e3fb91 136
palmdotax 0:f02641e3fb91 137 int main()
palmdotax 0:f02641e3fb91 138 {
palmdotax 9:86701fec3f79 139
palmdotax 6:9ed82a812ece 140
palmdotax 5:68b740d113e6 141 pc.baud(115200);
palmdotax 9:86701fec3f79 142 encoderA_1.rise(&EncoderA_1);
palmdotax 5:68b740d113e6 143 timerStart.start();
palmdotax 2:933d3edf38da 144 P1.setMode(1);
palmdotax 2:933d3edf38da 145 P1.setBias(0);
palmdotax 11:9df7ada37d31 146 // pc.printf("READY \n");
palmdotax 11:9df7ada37d31 147 pc.attach(&Rx_interrupt, Serial::RxIrq);
palmdotax 4:e6ab360e7de6 148 led1=1;
palmdotax 0:f02641e3fb91 149 while(1)
palmdotax 3:365615fa646e 150
palmdotax 11:9df7ada37d31 151 {
palmdotax 11:9df7ada37d31 152 timer_now=timerStart.read();
palmdotax 11:9df7ada37d31 153 if((timer_now-timer_later)>2)
palmdotax 11:9df7ada37d31 154 {
palmdotax 11:9df7ada37d31 155 m1.movespeed_1(1,0,0);
palmdotax 11:9df7ada37d31 156 m1.movespeed_2(1,0,0);
palmdotax 11:9df7ada37d31 157 }
palmdotax 11:9df7ada37d31 158 //shutdown.attach_us(&Timeout, 2.0);
palmdotax 12:8a06a803e373 159 else
palmdotax 12:8a06a803e373 160 {time_getsensor.attach_us(&getSensor, 2.0);
palmdotax 11:9df7ada37d31 161 time_distance.attach_us(&get_d, 2.0);
palmdotax 11:9df7ada37d31 162 PID_m1();
palmdotax 12:8a06a803e373 163 PID_m2();}
palmdotax 11:9df7ada37d31 164 }
palmdotax 0:f02641e3fb91 165 }