Zhixin Li / Mbed 2 deprecated PesL5Ex0a

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:cbabd9031b37, 2018-10-15 (annotated)

Committer:
Rhein
Date:
Mon Oct 15 09:41:49 2018 +0000
Revision:
0:cbabd9031b37
PES_Lecture5_Assignment

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Rhein 0:cbabd9031b37 1 #include "mbed.h"
Rhein 0:cbabd9031b37 2
Rhein 0:cbabd9031b37 3 DigitalOut gpo(D0);
Rhein 0:cbabd9031b37 4 DigitalOut led(LED_RED);
Rhein 0:cbabd9031b37 5 DigitalOut motor_dir(D7);
Rhein 0:cbabd9031b37 6
Rhein 0:cbabd9031b37 7 double Kp = 17.5;
Rhein 0:cbabd9031b37 8 double Ki = 1.02;
Rhein 0:cbabd9031b37 9 double Kd = 23.2;
Rhein 0:cbabd9031b37 10 double Ts = 0.01; //Sample time in seconds
Rhein 0:cbabd9031b37 11
Rhein 0:cbabd9031b37 12 double GetReferenceVelocity() //Determine reference value
Rhein 0:cbabd9031b37 13 {
Rhein 0:cbabd9031b37 14 const float maxVelocity = 8.4;
Rhein 0:cbabd9031b37 15 double referenceVelocity;
Rhein 0:cbabd9031b37 16
Rhein 0:cbabd9031b37 17 if (botton1)
Rhein 0:cbabd9031b37 18 {
Rhein 0:cbabd9031b37 19 referenceVelocity = PotMeterIn * maxVelocity;
Rhein 0:cbabd9031b37 20 }
Rhein 0:cbabd9031b37 21 else
Rhein 0:cbabd9031b37 22 {
Rhein 0:cbabd9031b37 23 referenceVelocity = -1*PotMeterIn * maxVelocity;
Rhein 0:cbabd9031b37 24 }
Rhein 0:cbabd9031b37 25 return referenceVelocity;
Rhein 0:cbabd9031b37 26
Rhein 0:cbabd9031b37 27 }
Rhein 0:cbabd9031b37 28
Rhein 0:cbabd9031b37 29 double GetActualPosition() // measure plant output
Rhein 0:cbabd9031b37 30 {
Rhein 0:cbabd9031b37 31 double pos_rad = - (pos_counts - pos_offset_counts) / (double)counts_per_rad;
Rhein 0:cbabd9031b37 32 return pos_rad;
Rhein 0:cbabd9031b37 33 }
Rhein 0:cbabd9031b37 34
Rhein 0:cbabd9031b37 35 double PID_controller(double error)
Rhein 0:cbabd9031b37 36 {
Rhein 0:cbabd9031b37 37 static double error_integral = 0;
Rhein 0:cbabd9031b37 38 static double error_prev = error;
Rhein 0:cbabd9031b37 39 static Biquad LowpassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241);
Rhein 0:cbabd9031b37 40
Rhein 0:cbabd9031b37 41 //P part
Rhein 0:cbabd9031b37 42 double u_k = Kp * e;
Rhein 0:cbabd9031b37 43
Rhein 0:cbabd9031b37 44 //I part
Rhein 0:cbabd9031b37 45 error_integral = error_integral + error * Ts;
Rhein 0:cbabd9031b37 46 double u_i = Ki * error_integral;
Rhein 0:cbabd9031b37 47
Rhein 0:cbabd9031b37 48 //D part
Rhein 0:cbabd9031b37 49 double error_derivative = (error - error_prev)/Ts;
Rhein 0:cbabd9031b37 50 double filtered_error_derivatice = LowPassFilter.step(error_derivative);
Rhein 0:cbabd9031b37 51 double u_d = Kd * filtered_error_derivative;
Rhein 0:cbabd9031b37 52 error_prev = error;
Rhein 0:cbabd9031b37 53
Rhein 0:cbabd9031b37 54 return u_k + u_i + u_d;
Rhein 0:cbabd9031b37 55 }