Roy van Zijl / Mbed 2 deprecated feed_forward_PI_copy

Werkend voor onze motor

Dependencies:   Encoder MODSERIAL QEI mbed

Fork of feed_forward_PI_copy by Dion de Greef

main.cpp@3:91e4ed7b6748, 2017-10-20 (annotated)

Committer:
RoyvZ
Date:
Fri Oct 20 10:20:59 2017 +0000
Revision:
3:91e4ed7b6748
Parent:
2:68de33d10c67
Child:
4:840ce3b21767
Werkend voor de motor;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RoyvZ 0:331597250051 1 #include "mbed.h"
RoyvZ 0:331597250051 2 #include "MODSERIAL.h"
RoyvZ 0:331597250051 3 #include "math.h"
DiondeGreef 2:68de33d10c67 4 #include "encoder.h"
RoyvZ 0:331597250051 5
RoyvZ 0:331597250051 6 DigitalOut motorDirection(D4);
DiondeGreef 1:92a60278860a 7 PwmOut motorSpeed(D5);
DiondeGreef 2:68de33d10c67 8 AnalogIn potMeterIn1(A1);
DiondeGreef 2:68de33d10c67 9 AnalogIn potMeterIn2(A2);
RoyvZ 3:91e4ed7b6748 10 InterruptIn button(D11);
DiondeGreef 2:68de33d10c67 11 Ticker m1_Ticker;
RoyvZ 3:91e4ed7b6748 12 Ticker SP_m1_Ticker;
DiondeGreef 2:68de33d10c67 13 Encoder encoder1(D13,D12);
RoyvZ 3:91e4ed7b6748 14 float M1_KP = 2.5;
RoyvZ 3:91e4ed7b6748 15 float M1_KI = 1.0;
RoyvZ 3:91e4ed7b6748 16 const float M1_TS = 0.01;
RoyvZ 3:91e4ed7b6748 17 const float SP_TS = 0.01;
RoyvZ 3:91e4ed7b6748 18 const float RAD_PER_PULSE = 0.000119;
RoyvZ 3:91e4ed7b6748 19 float m1_err_int = 0;
DiondeGreef 2:68de33d10c67 20 int motorD = 0;
RoyvZ 3:91e4ed7b6748 21 float motor1 = 0;
RoyvZ 3:91e4ed7b6748 22 float reference = 0;
RoyvZ 3:91e4ed7b6748 23 float position = 0;
RoyvZ 3:91e4ed7b6748 24 int outOfEncod = 0;
RoyvZ 3:91e4ed7b6748 25 int KP_changer = 1;
RoyvZ 3:91e4ed7b6748 26 float RotSpeed = 0;
RoyvZ 3:91e4ed7b6748 27
RoyvZ 3:91e4ed7b6748 28 int outOfEncod_SP_1 = 0;
RoyvZ 3:91e4ed7b6748 29 int outOfEncod_SP_2 = 0;
RoyvZ 3:91e4ed7b6748 30
RoyvZ 0:331597250051 31
RoyvZ 0:331597250051 32 MODSERIAL pc(USBTX, USBRX);
RoyvZ 0:331597250051 33
RoyvZ 0:331597250051 34
DiondeGreef 2:68de33d10c67 35 // Reusable PI controller
RoyvZ 3:91e4ed7b6748 36 float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ){
RoyvZ 3:91e4ed7b6748 37 e_int += Ts * e; // e_int is changed globally because it’s ’by reference’ (&)
RoyvZ 3:91e4ed7b6748 38 return Kp * e + Ki * e_int;
RoyvZ 3:91e4ed7b6748 39 }
RoyvZ 3:91e4ed7b6748 40
RoyvZ 3:91e4ed7b6748 41 void Change()
RoyvZ 3:91e4ed7b6748 42 {
RoyvZ 3:91e4ed7b6748 43 if(KP_changer == 60){
RoyvZ 3:91e4ed7b6748 44 KP_changer = 0;
RoyvZ 3:91e4ed7b6748 45 }
RoyvZ 3:91e4ed7b6748 46 else{
RoyvZ 3:91e4ed7b6748 47 KP_changer++;
RoyvZ 3:91e4ed7b6748 48 }
RoyvZ 3:91e4ed7b6748 49 }
RoyvZ 3:91e4ed7b6748 50
RoyvZ 3:91e4ed7b6748 51 void m1_Velocity(){
RoyvZ 3:91e4ed7b6748 52 outOfEncod_SP_1 = encoder1.getPosition();
RoyvZ 3:91e4ed7b6748 53 float RotDiff = (outOfEncod_SP_1 - outOfEncod_SP_2)*RAD_PER_PULSE;
RoyvZ 3:91e4ed7b6748 54 outOfEncod_SP_2 = outOfEncod_SP_1;
RoyvZ 3:91e4ed7b6748 55 RotSpeed = RotDiff/SP_TS;
RoyvZ 0:331597250051 56 }
DiondeGreef 2:68de33d10c67 57 // Next task, measure the error and apply the output to the plant
DiondeGreef 2:68de33d10c67 58 void m1_Controller() {
RoyvZ 3:91e4ed7b6748 59 reference = 5 * potMeterIn1.read();
RoyvZ 3:91e4ed7b6748 60 M1_KI = 2 * potMeterIn2.read();
RoyvZ 3:91e4ed7b6748 61 M1_KP = KP_changer*0.1;
RoyvZ 3:91e4ed7b6748 62 outOfEncod = encoder1.getPosition();
RoyvZ 3:91e4ed7b6748 63 position = RAD_PER_PULSE * outOfEncod;
RoyvZ 3:91e4ed7b6748 64 float ref_pos = reference - position; // Don’t use magic numbers!
RoyvZ 3:91e4ed7b6748 65 motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
RoyvZ 3:91e4ed7b6748 66 if (ref_pos <= 0)
RoyvZ 0:331597250051 67 {
RoyvZ 0:331597250051 68 //float motor1DirectionPin1 = 1;
DiondeGreef 2:68de33d10c67 69 motorD=1;
RoyvZ 0:331597250051 70 }
RoyvZ 0:331597250051 71 else
RoyvZ 0:331597250051 72 {
RoyvZ 0:331597250051 73 //float motor1DirectionPin1 = 0;
DiondeGreef 2:68de33d10c67 74 motorD=0;
RoyvZ 0:331597250051 75 }
RoyvZ 3:91e4ed7b6748 76 motorDirection.write(motorD);
RoyvZ 3:91e4ed7b6748 77 motorSpeed.write(motor1);
RoyvZ 0:331597250051 78 }
RoyvZ 0:331597250051 79
RoyvZ 0:331597250051 80 int main() {
RoyvZ 3:91e4ed7b6748 81 m1_Ticker.attach( &m1_Controller, M1_TS );
RoyvZ 3:91e4ed7b6748 82 SP_m1_Ticker.attach( &m1_Velocity, SP_TS ); // 100 Hz
RoyvZ 3:91e4ed7b6748 83 pc.baud(115200);
RoyvZ 3:91e4ed7b6748 84 pc.printf("Fuck you pc");
RoyvZ 3:91e4ed7b6748 85 m1_Controller();
RoyvZ 3:91e4ed7b6748 86 button.rise(&Change);
RoyvZ 3:91e4ed7b6748 87 while(true) {
RoyvZ 3:91e4ed7b6748 88 pc.printf("%f \t", RotSpeed);
RoyvZ 3:91e4ed7b6748 89 pc.printf("%f \t", M1_KI);
RoyvZ 3:91e4ed7b6748 90 pc.printf("%f \t", M1_KP);
RoyvZ 3:91e4ed7b6748 91 pc.printf("%f \t", position);
RoyvZ 3:91e4ed7b6748 92 pc.printf("%f \r\n", reference);
RoyvZ 3:91e4ed7b6748 93 //wait(0.5);
RoyvZ 0:331597250051 94 }
RoyvZ 0:331597250051 95 }
RoyvZ 3:91e4ed7b6748 96
RoyvZ 0:331597250051 97