Arnoud Meutstege / Mbed 2 deprecated 20170910_PID_V1_0

Working P loop

Dependencies:   MODSERIAL QEI mbed

main.cpp@0:85ca550760e9, 2017-10-09 (annotated)

Committer:
Arnoud113
Date:
Mon Oct 09 13:52:37 2017 +0000
Revision:
0:85ca550760e9
Child:
1:b66e14435f70
Working P Feedback loop

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arnoud113 0:85ca550760e9 1 #include "mbed.h"
Arnoud113 0:85ca550760e9 2 #include "QEI.h"
Arnoud113 0:85ca550760e9 3 #include "MODSERIAL.h"
Arnoud113 0:85ca550760e9 4 #include "math.h"
Arnoud113 0:85ca550760e9 5
Arnoud113 0:85ca550760e9 6
Arnoud113 0:85ca550760e9 7
Arnoud113 0:85ca550760e9 8 DigitalOut gpo(D0);
Arnoud113 0:85ca550760e9 9 DigitalOut ledb(LED_BLUE);
Arnoud113 0:85ca550760e9 10 DigitalOut ledr(LED_RED);
Arnoud113 0:85ca550760e9 11 DigitalOut ledg(LED_GREEN);
Arnoud113 0:85ca550760e9 12 DigitalOut motor1DC(D7);
Arnoud113 0:85ca550760e9 13 DigitalOut motor1PWM(D6);
Arnoud113 0:85ca550760e9 14 DigitalOut motor2DC(D4);
Arnoud113 0:85ca550760e9 15 DigitalOut motor2PWM(D5);
Arnoud113 0:85ca550760e9 16
Arnoud113 0:85ca550760e9 17 AnalogIn potMeter1(A0);
Arnoud113 0:85ca550760e9 18 AnalogIn potMeter2(A1);
Arnoud113 0:85ca550760e9 19 DigitalIn button1(D11);
Arnoud113 0:85ca550760e9 20 DigitalIn button2(D12);
Arnoud113 0:85ca550760e9 21 QEI Encoder1(D12,D13,NC,4200);
Arnoud113 0:85ca550760e9 22
Arnoud113 0:85ca550760e9 23 MODSERIAL pc(USBTX,USBRX);
Arnoud113 0:85ca550760e9 24
Arnoud113 0:85ca550760e9 25 Ticker controller;
Arnoud113 0:85ca550760e9 26
Arnoud113 0:85ca550760e9 27 const double pi = 3.1415926535897;
Arnoud113 0:85ca550760e9 28 const float MOTOR1_KP = 5;
Arnoud113 0:85ca550760e9 29 const float RAD_PER_PULSE = (2*pi)/4200;
Arnoud113 0:85ca550760e9 30 const float CONTROLLER_TS = 0.01; //TIME INTERVAL/ hZ
Arnoud113 0:85ca550760e9 31
Arnoud113 0:85ca550760e9 32 float P(double error, const float Kp){
Arnoud113 0:85ca550760e9 33 return Kp * error;
Arnoud113 0:85ca550760e9 34 }
Arnoud113 0:85ca550760e9 35
Arnoud113 0:85ca550760e9 36 void motor1_Controller(){
Arnoud113 0:85ca550760e9 37 double reference = 10*potMeter1.read();
Arnoud113 0:85ca550760e9 38 double position = RAD_PER_PULSE*Encoder1.getPulses();
Arnoud113 0:85ca550760e9 39 double motor1 = P(reference-position, MOTOR1_KP);
Arnoud113 0:85ca550760e9 40 motor1PWM = motor1;
Arnoud113 0:85ca550760e9 41
Arnoud113 0:85ca550760e9 42 if(motor1 > 0.1){
Arnoud113 0:85ca550760e9 43 motor1DC = 1;
Arnoud113 0:85ca550760e9 44
Arnoud113 0:85ca550760e9 45 ledr = 1;
Arnoud113 0:85ca550760e9 46 ledg = 1; //Blau
Arnoud113 0:85ca550760e9 47 ledb = 0;
Arnoud113 0:85ca550760e9 48 }
Arnoud113 0:85ca550760e9 49 else if (motor1<-0.1) {
Arnoud113 0:85ca550760e9 50 motor1DC = 0;
Arnoud113 0:85ca550760e9 51
Arnoud113 0:85ca550760e9 52 ledb = 1;
Arnoud113 0:85ca550760e9 53 ledr = 1;
Arnoud113 0:85ca550760e9 54 ledg = 0; //Groen
Arnoud113 0:85ca550760e9 55
Arnoud113 0:85ca550760e9 56 }
Arnoud113 0:85ca550760e9 57 else{
Arnoud113 0:85ca550760e9 58 motor1PWM = 0;
Arnoud113 0:85ca550760e9 59
Arnoud113 0:85ca550760e9 60 ledb = 1; //Rood
Arnoud113 0:85ca550760e9 61 ledr = 0;
Arnoud113 0:85ca550760e9 62 ledg = 1;
Arnoud113 0:85ca550760e9 63 }
Arnoud113 0:85ca550760e9 64
Arnoud113 0:85ca550760e9 65
Arnoud113 0:85ca550760e9 66 }
Arnoud113 0:85ca550760e9 67
Arnoud113 0:85ca550760e9 68 int main(){
Arnoud113 0:85ca550760e9 69 pc.baud(115200);
Arnoud113 0:85ca550760e9 70 controller.attach(&motor1_Controller, CONTROLLER_TS);
Arnoud113 0:85ca550760e9 71
Arnoud113 0:85ca550760e9 72 while(1){
Arnoud113 0:85ca550760e9 73 double reference = 10*potMeter1.read();
Arnoud113 0:85ca550760e9 74 double position = RAD_PER_PULSE*Encoder1.getPulses();
Arnoud113 0:85ca550760e9 75 double motor1 = P(reference-position, MOTOR1_KP);
Arnoud113 0:85ca550760e9 76 pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position);
Arnoud113 0:85ca550760e9 77 }
Arnoud113 0:85ca550760e9 78 }