Ingmar Loohuis / Mbed 2 deprecated MotorControl

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:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@13:f92e918af729, 2016-10-24 (annotated)

Committer:
IngmarLoohuis
Date:
Mon Oct 24 14:28:04 2016 +0000
Revision:
13:f92e918af729
Parent:
12:0cf4e70f6b8e
Child:
14:6ecf2b986a4b
PIDF works, values Annelies

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IngmarLoohuis 0:2f40eb89ffce 1 #include "mbed.h"
IngmarLoohuis 2:665df4abd084 2 #include "MODSERIAL.h"
IngmarLoohuis 0:2f40eb89ffce 3 #include "QEI.h"
IngmarLoohuis 0:2f40eb89ffce 4 #include "math.h"
IngmarLoohuis 12:0cf4e70f6b8e 5 #include "BiQuad.h"
IngmarLoohuis 11:eda4fbf91948 6 //#include "HIDScope.h"
IngmarLoohuis 6:6bc6ce1fe94e 7
IngmarLoohuis 6:6bc6ce1fe94e 8 //*****************Defining ports********************
IngmarLoohuis 2:665df4abd084 9 DigitalOut motor1DirectionPin (D4);
IngmarLoohuis 1:f26a53da33ed 10 PwmOut motor1MagnitudePin(D5);
IngmarLoohuis 2:665df4abd084 11 DigitalIn button(D2);
IngmarLoohuis 1:f26a53da33ed 12 Serial pc(USBTX,USBRX);
IngmarLoohuis 4:30d8610b63a6 13 QEI encoder(D12,D13,NC,32);
IngmarLoohuis 11:eda4fbf91948 14 //HIDScope scope(2);
IngmarLoohuis 8:7cc4d6d9c2b5 15 DigitalOut led(D0);
IngmarLoohuis 6:6bc6ce1fe94e 16 //*******************Setting tickers and printers*******************
IngmarLoohuis 2:665df4abd084 17 Ticker callMotor;
IngmarLoohuis 6:6bc6ce1fe94e 18 Ticker angPos;
IngmarLoohuis 6:6bc6ce1fe94e 19 Ticker t1;
IngmarLoohuis 7:742b1969f6c9 20 Ticker t2;
IngmarLoohuis 7:742b1969f6c9 21 Ticker t3;
IngmarLoohuis 7:742b1969f6c9 22 Ticker myControllerTicker;
IngmarLoohuis 11:eda4fbf91948 23 Ticker scopeTimer;
IngmarLoohuis 6:6bc6ce1fe94e 24 //**************Go flags********************************************
IngmarLoohuis 6:6bc6ce1fe94e 25 volatile bool fn1_go = false;
IngmarLoohuis 6:6bc6ce1fe94e 26 void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
IngmarLoohuis 7:742b1969f6c9 27 volatile bool fn2_go = false;
IngmarLoohuis 7:742b1969f6c9 28 void fn2_activate(){ fn2_go = true; }; //Activates the go-flag
IngmarLoohuis 7:742b1969f6c9 29 volatile bool fn3_go = false;
IngmarLoohuis 7:742b1969f6c9 30 void fn3_activate(){ fn3_go = true; }; //Activates the go-flag
IngmarLoohuis 6:6bc6ce1fe94e 31
IngmarLoohuis 6:6bc6ce1fe94e 32 //***************Global Variables***********************************
IngmarLoohuis 6:6bc6ce1fe94e 33 const double pi = 3.14159265359;
IngmarLoohuis 11:eda4fbf91948 34 double m1_ts = 0.01;
IngmarLoohuis 6:6bc6ce1fe94e 35 const int velocityChannel = 0;
IngmarLoohuis 6:6bc6ce1fe94e 36 const double transmissionShoulder =94.4/40.2;
IngmarLoohuis 6:6bc6ce1fe94e 37 const double transmissionElbow = 1.0;
IngmarLoohuis 12:0cf4e70f6b8e 38 // controller constants
IngmarLoohuis 13:f92e918af729 39 const double m1_Kp = 0.000048765659063912, m1_Ki = 0.0000228295351674407, m1_Kd = 0.0000255784613247063, m1_N = 54.5397025421619;
IngmarLoohuis 12:0cf4e70f6b8e 40 double m1_v1 = 0, m1_v2 = 0; // Memory variables
IngmarLoohuis 13:f92e918af729 41 const double m1_Ts = 0.001; // Controller sample time
IngmarLoohuis 12:0cf4e70f6b8e 42 double v1 = 0;
IngmarLoohuis 12:0cf4e70f6b8e 43 double v2 = 0;
IngmarLoohuis 12:0cf4e70f6b8e 44 // position variable
IngmarLoohuis 8:7cc4d6d9c2b5 45 volatile double radians;
IngmarLoohuis 12:0cf4e70f6b8e 46 //plant variable
IngmarLoohuis 9:6a065971d0ae 47 volatile double motor1;
IngmarLoohuis 12:0cf4e70f6b8e 48
IngmarLoohuis 12:0cf4e70f6b8e 49
IngmarLoohuis 6:6bc6ce1fe94e 50 //*****************Angles Arms***********************
IngmarLoohuis 1:f26a53da33ed 51
IngmarLoohuis 6:6bc6ce1fe94e 52 double O1=1.7633;
IngmarLoohuis 6:6bc6ce1fe94e 53 double O2=2.0915;
IngmarLoohuis 6:6bc6ce1fe94e 54 double O3=1.8685;
IngmarLoohuis 6:6bc6ce1fe94e 55 double O4=1.1363;
IngmarLoohuis 6:6bc6ce1fe94e 56 double O5=2.3960;
IngmarLoohuis 6:6bc6ce1fe94e 57 double O6=2.0827;
IngmarLoohuis 6:6bc6ce1fe94e 58 double B1=1.3551;
IngmarLoohuis 6:6bc6ce1fe94e 59 double B2=0.5964;
IngmarLoohuis 6:6bc6ce1fe94e 60 double B3=0.06652;
IngmarLoohuis 6:6bc6ce1fe94e 61 double B4=0.0669;
IngmarLoohuis 6:6bc6ce1fe94e 62 double B5=1.7462;
IngmarLoohuis 6:6bc6ce1fe94e 63 double B6=-0.8994;
IngmarLoohuis 5:931594a366b7 64
IngmarLoohuis 6:6bc6ce1fe94e 65 //**********functions******************************
IngmarLoohuis 12:0cf4e70f6b8e 66 double PID( double err, const double Kp, const double Ki, const double Kd,
IngmarLoohuis 12:0cf4e70f6b8e 67 const double Ts, const double N, double &v1, double &v2 ) {
IngmarLoohuis 12:0cf4e70f6b8e 68 // These variables are only calculated once!
IngmarLoohuis 12:0cf4e70f6b8e 69 const double a1 = (-4.0/(N*Ts+2));
IngmarLoohuis 12:0cf4e70f6b8e 70 const double a2 = -(N*Ts-2)/(N*Ts+2);
IngmarLoohuis 12:0cf4e70f6b8e 71 const double b0 = (4.0*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
IngmarLoohuis 12:0cf4e70f6b8e 72 const double b1 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
IngmarLoohuis 12:0cf4e70f6b8e 73 const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
IngmarLoohuis 12:0cf4e70f6b8e 74
IngmarLoohuis 12:0cf4e70f6b8e 75 double v = err - a1*v1 - a2*v2;
IngmarLoohuis 12:0cf4e70f6b8e 76 double u = b0*v + b1*v1 + b2*v2;
IngmarLoohuis 12:0cf4e70f6b8e 77 v2 = v1; v1 = v;
IngmarLoohuis 12:0cf4e70f6b8e 78 return u;
IngmarLoohuis 12:0cf4e70f6b8e 79 }
IngmarLoohuis 12:0cf4e70f6b8e 80
IngmarLoohuis 9:6a065971d0ae 81
IngmarLoohuis 8:7cc4d6d9c2b5 82 void getAngPosition()
IngmarLoohuis 6:6bc6ce1fe94e 83 {
IngmarLoohuis 6:6bc6ce1fe94e 84 volatile int pulses = encoder.getPulses();
IngmarLoohuis 9:6a065971d0ae 85 radians = (pulses / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
IngmarLoohuis 11:eda4fbf91948 86 // scope.set(velocityChannel,pulses);
IngmarLoohuis 11:eda4fbf91948 87 // scope.send();
IngmarLoohuis 13:f92e918af729 88
IngmarLoohuis 11:eda4fbf91948 89
IngmarLoohuis 6:6bc6ce1fe94e 90 }
IngmarLoohuis 6:6bc6ce1fe94e 91
IngmarLoohuis 7:742b1969f6c9 92 // Next task, measure the error and apply the output to the plant
IngmarLoohuis 8:7cc4d6d9c2b5 93 void motor1_Controller(double radians)
IngmarLoohuis 7:742b1969f6c9 94 {
IngmarLoohuis 13:f92e918af729 95 double reference = 0.5*pi;
IngmarLoohuis 8:7cc4d6d9c2b5 96 volatile double error1 = reference - radians;
IngmarLoohuis 12:0cf4e70f6b8e 97 motor1 = PID( error1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, v1, v2 );
IngmarLoohuis 12:0cf4e70f6b8e 98 //pc.printf("%d\n\r",motor1);
IngmarLoohuis 12:0cf4e70f6b8e 99 //scope.set(velocityChannel,motor1);
IngmarLoohuis 9:6a065971d0ae 100 //scope.send();
IngmarLoohuis 13:f92e918af729 101 pc.printf("error1= %d\n\r",error1);
IngmarLoohuis 7:742b1969f6c9 102 }
IngmarLoohuis 7:742b1969f6c9 103
IngmarLoohuis 7:742b1969f6c9 104
IngmarLoohuis 10:54b66bd1db20 105 void control(double motor1)
IngmarLoohuis 4:30d8610b63a6 106 {
IngmarLoohuis 13:f92e918af729 107 if(abs(motor1)>0.000015)
IngmarLoohuis 2:665df4abd084 108 {
IngmarLoohuis 9:6a065971d0ae 109 motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
IngmarLoohuis 2:665df4abd084 110 }
IngmarLoohuis 9:6a065971d0ae 111 else
IngmarLoohuis 2:665df4abd084 112 {
IngmarLoohuis 6:6bc6ce1fe94e 113 motor1MagnitudePin=0.0;
IngmarLoohuis 2:665df4abd084 114 }
IngmarLoohuis 9:6a065971d0ae 115 if(motor1<=0)
IngmarLoohuis 9:6a065971d0ae 116 {
IngmarLoohuis 9:6a065971d0ae 117 motor1DirectionPin=0.0;
IngmarLoohuis 9:6a065971d0ae 118 }
IngmarLoohuis 9:6a065971d0ae 119 else {
IngmarLoohuis 9:6a065971d0ae 120 motor1DirectionPin=1.0;
IngmarLoohuis 9:6a065971d0ae 121 }
IngmarLoohuis 0:2f40eb89ffce 122 }
IngmarLoohuis 7:742b1969f6c9 123
IngmarLoohuis 6:6bc6ce1fe94e 124 //****************MAIN FUNCTION*********************************
IngmarLoohuis 0:2f40eb89ffce 125 int main()
IngmarLoohuis 8:7cc4d6d9c2b5 126 {motor1MagnitudePin.period(1.0/1000.0);
IngmarLoohuis 13:f92e918af729 127 t1.attach(&fn1_activate, 0.0001f);
IngmarLoohuis 7:742b1969f6c9 128 t2.attach(&fn2_activate, 0.0001f);
IngmarLoohuis 7:742b1969f6c9 129 t3.attach(&fn3_activate, 0.0001f);
IngmarLoohuis 11:eda4fbf91948 130 pc.baud(115200);
IngmarLoohuis 10:54b66bd1db20 131 while(true)
IngmarLoohuis 10:54b66bd1db20 132 {
IngmarLoohuis 6:6bc6ce1fe94e 133 if(fn1_go)
IngmarLoohuis 6:6bc6ce1fe94e 134 {
IngmarLoohuis 6:6bc6ce1fe94e 135 fn1_go = false;
IngmarLoohuis 10:54b66bd1db20 136 control(motor1);
IngmarLoohuis 6:6bc6ce1fe94e 137 }
IngmarLoohuis 7:742b1969f6c9 138 if(fn2_go)
IngmarLoohuis 7:742b1969f6c9 139 {
IngmarLoohuis 7:742b1969f6c9 140 fn2_go = false;
IngmarLoohuis 8:7cc4d6d9c2b5 141 motor1_Controller(radians);
IngmarLoohuis 7:742b1969f6c9 142 }
IngmarLoohuis 7:742b1969f6c9 143 if(fn3_go)
IngmarLoohuis 7:742b1969f6c9 144 {
IngmarLoohuis 7:742b1969f6c9 145 fn3_go = false;
IngmarLoohuis 7:742b1969f6c9 146 getAngPosition();
IngmarLoohuis 7:742b1969f6c9 147 }
IngmarLoohuis 7:742b1969f6c9 148 }
IngmarLoohuis 2:665df4abd084 149 }