Betago / Mbed 2 deprecated BETAGO

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?

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