10/25/2015
Dependencies: PID mbed MaxSonar eeprom iSerial Fork_Boss_Communication_Robot
main.cpp@7:2daffd310c71, 2015-10-27 (annotated)
- Committer:
- palmdotax
- Date:
- Tue Oct 27 06:27:56 2015 +0000
- Revision:
- 7:2daffd310c71
- Parent:
- 6:9ed82a812ece
- Child:
- 8:8fbc0c858875
27/10/2015
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:7c3dbf140bfc | 4 | |
palmdotax | 3:365615fa646e | 5 | DigitalOut led1(LED1); |
palmdotax | 4:e6ab360e7de6 | 6 | InterruptIn encoderA(D10); |
palmdotax | 4:e6ab360e7de6 | 7 | InterruptIn encoderB(D11); |
palmdotax | 2:933d3edf38da | 8 | Timer timerStart; |
palmdotax | 3:365615fa646e | 9 | Timeout timecount; |
palmdotax | 1:7c3dbf140bfc | 10 | move m1; |
palmdotax | 4:e6ab360e7de6 | 11 | PID P1(0.005,0.005,0,0.1); |
palmdotax | 7:2daffd310c71 | 12 | double setp=0; |
palmdotax | 0:f02641e3fb91 | 13 | //DigitalIn encoderB(D5); |
palmdotax | 0:f02641e3fb91 | 14 | |
palmdotax | 0:f02641e3fb91 | 15 | |
palmdotax | 0:f02641e3fb91 | 16 | Serial pc(SERIAL_TX,SERIAL_RX); |
palmdotax | 0:f02641e3fb91 | 17 | int Encoderpos = 0; |
palmdotax | 6:9ed82a812ece | 18 | float valocity =0,pulse=0,count=0,r=0.125,velocityreal=0; |
palmdotax | 3:365615fa646e | 19 | float outPID =0; |
palmdotax | 3:365615fa646e | 20 | //double Input,Output,setp,Kp=0.005,Ki=0.005,Kd=0; |
palmdotax | 3:365615fa646e | 21 | |
palmdotax | 0:f02641e3fb91 | 22 | |
palmdotax | 0:f02641e3fb91 | 23 | void EncoderA() |
palmdotax | 0:f02641e3fb91 | 24 | { if(encoderB==0) |
palmdotax | 0:f02641e3fb91 | 25 | { Encoderpos = Encoderpos + 1;} |
palmdotax | 0:f02641e3fb91 | 26 | else |
palmdotax | 0:f02641e3fb91 | 27 | { Encoderpos = Encoderpos -1;} |
palmdotax | 1:7c3dbf140bfc | 28 | |
palmdotax | 0:f02641e3fb91 | 29 | //Encoderpos = Encoderpos + 1; |
palmdotax | 3:365615fa646e | 30 | // valocity+=1; |
palmdotax | 1:7c3dbf140bfc | 31 | // pc.printf("%d \n",Encoderpos); |
palmdotax | 3:365615fa646e | 32 | pulse+=1; |
palmdotax | 5:68b740d113e6 | 33 | //pc.printf("pulse=%f \n",pulse); |
palmdotax | 5:68b740d113e6 | 34 | // if(pulse==128) |
palmdotax | 5:68b740d113e6 | 35 | //{count+=1;pulse=0; pc.printf("count=%f \n",count);} |
palmdotax | 5:68b740d113e6 | 36 | |
palmdotax | 0:f02641e3fb91 | 37 | } |
palmdotax | 0:f02641e3fb91 | 38 | |
palmdotax | 0:f02641e3fb91 | 39 | |
palmdotax | 3:365615fa646e | 40 | /*void EncoderB() |
palmdotax | 0:f02641e3fb91 | 41 | { |
palmdotax | 0:f02641e3fb91 | 42 | if(encoderA==1) |
palmdotax | 0:f02641e3fb91 | 43 | { Encoderpos = Encoderpos + 1;} |
palmdotax | 0:f02641e3fb91 | 44 | else |
palmdotax | 0:f02641e3fb91 | 45 | { Encoderpos = Encoderpos -1;} |
palmdotax | 0:f02641e3fb91 | 46 | pc.printf("%d",Encoderpos); |
palmdotax | 3:365615fa646e | 47 | }*/ |
palmdotax | 5:68b740d113e6 | 48 | void getvelo() |
palmdotax | 3:365615fa646e | 49 | { |
palmdotax | 6:9ed82a812ece | 50 | valocity=pulse*((2*3.14*r)/128); |
palmdotax | 7:2daffd310c71 | 51 | pc.printf("valocity=%f \n",valocity); |
palmdotax | 3:365615fa646e | 52 | count=0; |
palmdotax | 4:e6ab360e7de6 | 53 | timerStart.reset(); |
palmdotax | 0:f02641e3fb91 | 54 | } |
palmdotax | 7:2daffd310c71 | 55 | double map(double x, double in_min, double in_max, double out_min, double out_max) |
palmdotax | 5:68b740d113e6 | 56 | { |
palmdotax | 6:9ed82a812ece | 57 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
palmdotax | 5:68b740d113e6 | 58 | |
palmdotax | 5:68b740d113e6 | 59 | } |
palmdotax | 0:f02641e3fb91 | 60 | |
palmdotax | 0:f02641e3fb91 | 61 | int main() |
palmdotax | 0:f02641e3fb91 | 62 | { |
palmdotax | 5:68b740d113e6 | 63 | int times=0; |
palmdotax | 6:9ed82a812ece | 64 | |
palmdotax | 5:68b740d113e6 | 65 | pc.baud(115200); |
palmdotax | 5:68b740d113e6 | 66 | encoderA.rise(&EncoderA); |
palmdotax | 5:68b740d113e6 | 67 | timerStart.start(); |
palmdotax | 2:933d3edf38da | 68 | P1.setMode(1); |
palmdotax | 6:9ed82a812ece | 69 | //setp=map(1,0,1.0916,0,1); |
palmdotax | 6:9ed82a812ece | 70 | setp=map(1.0,0.0,1.094,0.0,1.0); |
palmdotax | 6:9ed82a812ece | 71 | |
palmdotax | 6:9ed82a812ece | 72 | // P1.setSetPoint(velocityreal);//setpont |
palmdotax | 7:2daffd310c71 | 73 | P1.setSetPoint(setp);//setpont |
palmdotax | 2:933d3edf38da | 74 | P1.setBias(0); |
palmdotax | 3:365615fa646e | 75 | pc.printf("READY \n"); |
palmdotax | 6:9ed82a812ece | 76 | |
palmdotax | 4:e6ab360e7de6 | 77 | led1=1; |
palmdotax | 0:f02641e3fb91 | 78 | while(1) |
palmdotax | 3:365615fa646e | 79 | |
palmdotax | 2:933d3edf38da | 80 | { |
palmdotax | 7:2daffd310c71 | 81 | // pc.printf("%f \n",setp); |
palmdotax | 5:68b740d113e6 | 82 | times=timerStart.read(); |
palmdotax | 5:68b740d113e6 | 83 | if(times==1)// m/s |
palmdotax | 5:68b740d113e6 | 84 | { |
palmdotax | 5:68b740d113e6 | 85 | getvelo(); |
palmdotax | 5:68b740d113e6 | 86 | //pc.printf("TIME \n"); |
palmdotax | 5:68b740d113e6 | 87 | times=0; |
palmdotax | 6:9ed82a812ece | 88 | pulse=0; |
palmdotax | 4:e6ab360e7de6 | 89 | } |
palmdotax | 6:9ed82a812ece | 90 | |
palmdotax | 4:e6ab360e7de6 | 91 | P1.setProcessValue(valocity); |
palmdotax | 3:365615fa646e | 92 | // m1.movespeed(1,1.0); |
palmdotax | 4:e6ab360e7de6 | 93 | outPID=P1.compute(); |
palmdotax | 7:2daffd310c71 | 94 | pc.printf("outPID=%f \n",outPID); |
palmdotax | 7:2daffd310c71 | 95 | m1.movespeed(1,setp,outPID); |
palmdotax | 7:2daffd310c71 | 96 | // m1.movespeed(1,1,0); |
palmdotax | 4:e6ab360e7de6 | 97 | |
palmdotax | 2:933d3edf38da | 98 | wait(0.1); |
palmdotax | 1:7c3dbf140bfc | 99 | |
palmdotax | 7:2daffd310c71 | 100 | |
palmdotax | 0:f02641e3fb91 | 101 | |
palmdotax | 0:f02641e3fb91 | 102 | } |
palmdotax | 0:f02641e3fb91 | 103 | } |