Margreeth de Breij / Mbed 2 deprecated frdm_Motor_V2_3

Motor programma

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_3 by Robert Schulte

main.cpp@11:0793a78109a2, 2015-09-29 (annotated)

Committer:
Rvs94
Date:
Tue Sep 29 14:01:47 2015 +0000
Revision:
11:0793a78109a2
Parent:
10:80fe931a71e4
Child:
12:69ab81cf5b7d
Werkende PI controller, zonder filtering. Volgende stap is filtering en implementatie PID controller. Kp = 0.07, Ki = 0.005

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Margreeth95 0:284ed397e046 1 #include "mbed.h"
Margreeth95 0:284ed397e046 2 #include "MODSERIAL.h"
Margreeth95 0:284ed397e046 3 #include "HIDScope.h"
Margreeth95 0:284ed397e046 4 #include "QEI.h"
Margreeth95 0:284ed397e046 5
Margreeth95 0:284ed397e046 6 Serial pc(USBTX, USBRX); // tx, rx
Margreeth95 0:284ed397e046 7 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
Margreeth95 0:284ed397e046 8 PwmOut motor2speed(D5);
Margreeth95 0:284ed397e046 9 DigitalIn button1(SW3);
Rvs94 9:774fc3c6a39e 10 DigitalIn encoderA(D3);
Rvs94 9:774fc3c6a39e 11 DigitalIn encoderB(D2);
Rvs94 9:774fc3c6a39e 12 AnalogIn potmeter2(A5);
Rvs94 5:455773cf460b 13 QEI Encoder(D3, D2, NC, 128);
Rvs94 1:48aba8d5610a 14 HIDScope scope(3);
Margreeth95 0:284ed397e046 15 Ticker ScopeTime;
Rvs94 9:774fc3c6a39e 16 Ticker myControllerTicker;
Rvs94 7:67b50d4fb03c 17
Rvs94 7:67b50d4fb03c 18 double Aantal_Degs;
Rvs94 7:67b50d4fb03c 19 double Aantal_pulses;
Rvs94 10:80fe931a71e4 20 double reference;
Rvs94 10:80fe931a71e4 21 double position;
Rvs94 2:099da0fc31b6 22
Margreeth95 0:284ed397e046 23 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Margreeth95 0:284ed397e046 24 {
Margreeth95 0:284ed397e046 25 scope.set(0, motor2direction.read());
Margreeth95 0:284ed397e046 26 scope.set(1, motor2speed.read());
Rvs94 1:48aba8d5610a 27 scope.set(2, Aantal_Degs);
Rvs94 10:80fe931a71e4 28 Aantal_Degs = Encoder.getPulses()*360/(0.5*128*131);
Rvs94 4:0d4aff8b57b3 29
Margreeth95 0:284ed397e046 30 scope.send();
Rvs94 1:48aba8d5610a 31
Margreeth95 0:284ed397e046 32 }
Rvs94 9:774fc3c6a39e 33 // Controller gain
Rvs94 11:0793a78109a2 34 const double m2_Kp = 0.07,m2_Ki = 0.005;
Rvs94 11:0793a78109a2 35 const double m2_Ts = 0.01;
Rvs94 11:0793a78109a2 36 double m2_err_int = 0;
Rvs94 10:80fe931a71e4 37
Rvs94 11:0793a78109a2 38 // Reusable PI controller
Rvs94 11:0793a78109a2 39 double P( double error, const double Kp, const double Ki, double Ts, double &e_int)
Rvs94 9:774fc3c6a39e 40 {
Rvs94 11:0793a78109a2 41 e_int = e_int + Ts*error;
Rvs94 11:0793a78109a2 42 return Kp * error + Ki*e_int;
Rvs94 9:774fc3c6a39e 43 }
Margreeth95 0:284ed397e046 44
Rvs94 9:774fc3c6a39e 45 // Next task, measure the error and apply the output to the plant
Rvs94 11:0793a78109a2 46 void motor2_Controller()
Rvs94 9:774fc3c6a39e 47 {
Rvs94 10:80fe931a71e4 48 reference = potmeter2.read()*360;
Rvs94 10:80fe931a71e4 49 position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs
Rvs94 11:0793a78109a2 50 double P2 = P( reference - position, m2_Kp, m2_Ki, m2_Ts, m2_err_int);
Rvs94 9:774fc3c6a39e 51 motor2speed = abs(P2);
Rvs94 9:774fc3c6a39e 52 if(P2 > 0)
Rvs94 9:774fc3c6a39e 53 {
Rvs94 9:774fc3c6a39e 54 motor2direction = 0;
Rvs94 9:774fc3c6a39e 55 }
Rvs94 9:774fc3c6a39e 56 else
Rvs94 9:774fc3c6a39e 57 {
Rvs94 9:774fc3c6a39e 58 motor2direction = 1;
Rvs94 9:774fc3c6a39e 59 }
Rvs94 9:774fc3c6a39e 60
Rvs94 9:774fc3c6a39e 61 }
Rvs94 3:687729d7996e 62
Margreeth95 0:284ed397e046 63 int main()
Rvs94 9:774fc3c6a39e 64 {
Margreeth95 0:284ed397e046 65 pc.baud(115200);
Rvs94 3:687729d7996e 66 pc.printf("Tot aan loop werkt\n");
Rvs94 9:774fc3c6a39e 67
Margreeth95 0:284ed397e046 68 ScopeTime.attach_us(&ScopeSend, 10e4);
Rvs94 11:0793a78109a2 69 myControllerTicker.attach( &motor2_Controller, 0.01f ); // 100 Hz
Rvs94 9:774fc3c6a39e 70 while(true)
Margreeth95 0:284ed397e046 71 {
Rvs94 10:80fe931a71e4 72 pc.printf("position = %f aantal degs = %f \n",reference,position);
Rvs94 10:80fe931a71e4 73 wait(0.2f);
Margreeth95 0:284ed397e046 74 }
Rvs94 9:774fc3c6a39e 75
Margreeth95 0:284ed397e046 76 }