10/25/2015
Dependencies: PID mbed MaxSonar eeprom iSerial Fork_Boss_Communication_Robot
main.cpp@2:933d3edf38da, 2015-10-22 (annotated)
- Committer:
- palmdotax
- Date:
- Thu Oct 22 11:58:57 2015 +0000
- Revision:
- 2:933d3edf38da
- Parent:
- 1:7c3dbf140bfc
- Child:
- 3:365615fa646e
v2
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 | 2:933d3edf38da | 4 | //#include "pidmotor.h" |
palmdotax | 1:7c3dbf140bfc | 5 | |
palmdotax | 0:f02641e3fb91 | 6 | InterruptIn encoderA(D6); |
palmdotax | 0:f02641e3fb91 | 7 | InterruptIn encoderB(D5); |
palmdotax | 2:933d3edf38da | 8 | Timer timerStart; |
palmdotax | 1:7c3dbf140bfc | 9 | move m1; |
palmdotax | 2:933d3edf38da | 10 | PID P1(0.001,0,0,0.1); |
palmdotax | 0:f02641e3fb91 | 11 | |
palmdotax | 0:f02641e3fb91 | 12 | //DigitalIn encoderB(D5); |
palmdotax | 0:f02641e3fb91 | 13 | |
palmdotax | 0:f02641e3fb91 | 14 | |
palmdotax | 0:f02641e3fb91 | 15 | Serial pc(SERIAL_TX,SERIAL_RX); |
palmdotax | 0:f02641e3fb91 | 16 | int Encoderpos = 0; |
palmdotax | 1:7c3dbf140bfc | 17 | float valocity =0; |
palmdotax | 2:933d3edf38da | 18 | float outPID =0,i=0; |
palmdotax | 2:933d3edf38da | 19 | double Input,Output,setp,Kp=0.005,Ki=0.005,Kd=0; |
palmdotax | 2:933d3edf38da | 20 | // pid1 p(&Input,&Output,&setp,Kp,Ki,Kd,1); |
palmdotax | 0:f02641e3fb91 | 21 | |
palmdotax | 0:f02641e3fb91 | 22 | void EncoderA() |
palmdotax | 0:f02641e3fb91 | 23 | { if(encoderB==0) |
palmdotax | 0:f02641e3fb91 | 24 | { Encoderpos = Encoderpos + 1;} |
palmdotax | 0:f02641e3fb91 | 25 | else |
palmdotax | 0:f02641e3fb91 | 26 | { Encoderpos = Encoderpos -1;} |
palmdotax | 1:7c3dbf140bfc | 27 | |
palmdotax | 0:f02641e3fb91 | 28 | //Encoderpos = Encoderpos + 1; |
palmdotax | 0:f02641e3fb91 | 29 | valocity+=1; |
palmdotax | 1:7c3dbf140bfc | 30 | // pc.printf("%d \n",Encoderpos); |
palmdotax | 0:f02641e3fb91 | 31 | } |
palmdotax | 0:f02641e3fb91 | 32 | |
palmdotax | 0:f02641e3fb91 | 33 | |
palmdotax | 0:f02641e3fb91 | 34 | void EncoderB() |
palmdotax | 0:f02641e3fb91 | 35 | { |
palmdotax | 0:f02641e3fb91 | 36 | if(encoderA==1) |
palmdotax | 0:f02641e3fb91 | 37 | { Encoderpos = Encoderpos + 1;} |
palmdotax | 0:f02641e3fb91 | 38 | else |
palmdotax | 0:f02641e3fb91 | 39 | { Encoderpos = Encoderpos -1;} |
palmdotax | 0:f02641e3fb91 | 40 | pc.printf("%d",Encoderpos); |
palmdotax | 0:f02641e3fb91 | 41 | } |
palmdotax | 0:f02641e3fb91 | 42 | |
palmdotax | 0:f02641e3fb91 | 43 | int main() |
palmdotax | 0:f02641e3fb91 | 44 | { |
palmdotax | 1:7c3dbf140bfc | 45 | |
palmdotax | 0:f02641e3fb91 | 46 | pc.baud(115200); |
palmdotax | 0:f02641e3fb91 | 47 | encoderA.rise(&EncoderA); |
palmdotax | 2:933d3edf38da | 48 | timerStart.start(); |
palmdotax | 2:933d3edf38da | 49 | |
palmdotax | 2:933d3edf38da | 50 | |
palmdotax | 2:933d3edf38da | 51 | P1.setMode(1); |
palmdotax | 2:933d3edf38da | 52 | P1.setProcessValue(0.7); |
palmdotax | 1:7c3dbf140bfc | 53 | P1.setSetPoint(0.8); |
palmdotax | 2:933d3edf38da | 54 | P1.setBias(0); |
palmdotax | 0:f02641e3fb91 | 55 | while(1) |
palmdotax | 2:933d3edf38da | 56 | { |
palmdotax | 2:933d3edf38da | 57 | m1.movespeed(1,1.0); |
palmdotax | 2:933d3edf38da | 58 | outPID=P1.compute(); |
palmdotax | 1:7c3dbf140bfc | 59 | pc.printf("%f \n",outPID); |
palmdotax | 2:933d3edf38da | 60 | wait(0.1); |
palmdotax | 1:7c3dbf140bfc | 61 | |
palmdotax | 0:f02641e3fb91 | 62 | // pc.printf("%d \n",Encoderpos); |
palmdotax | 0:f02641e3fb91 | 63 | |
palmdotax | 0:f02641e3fb91 | 64 | } |
palmdotax | 0:f02641e3fb91 | 65 | } |