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@16:2083f634c91c, 2016-10-25 (annotated)

Committer:
IngmarLoohuis
Date:
Tue Oct 25 14:18:30 2016 +0000
Revision:
16:2083f634c91c
Parent:
15:d38d5d4ae86a 
KIES VORIGE COMMIT, MAAK 2 APARTE PID CONTROLLERS

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 16:2083f634c91c 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 15:d38d5d4ae86a 11 DigitalOut motor2DirectionPin (D6);
IngmarLoohuis 15:d38d5d4ae86a 12 PwmOut motor2MagnitudePin(D7);
IngmarLoohuis 15:d38d5d4ae86a 13 QEI encoder_m1(D12,D13,NC,32);
IngmarLoohuis 15:d38d5d4ae86a 14 QEI encoder_m2(D10,D11,NC,32);
IngmarLoohuis 16:2083f634c91c 15 HIDScope scope(2);
IngmarLoohuis 2:665df4abd084 16 DigitalIn button(D2);
IngmarLoohuis 1:f26a53da33ed 17 Serial pc(USBTX,USBRX);
IngmarLoohuis 6:6bc6ce1fe94e 18 //*******************Setting tickers and printers*******************
IngmarLoohuis 15:d38d5d4ae86a 19 Ticker angPos1;
IngmarLoohuis 6:6bc6ce1fe94e 20 Ticker t1;
IngmarLoohuis 7:742b1969f6c9 21 Ticker t2;
IngmarLoohuis 7:742b1969f6c9 22 Ticker t3;
IngmarLoohuis 15:d38d5d4ae86a 23 Ticker t4;
IngmarLoohuis 15:d38d5d4ae86a 24 Ticker t5;
IngmarLoohuis 15:d38d5d4ae86a 25 Ticker t6;
IngmarLoohuis 16:2083f634c91c 26 Ticker HIDS;
IngmarLoohuis 15:d38d5d4ae86a 27
IngmarLoohuis 15:d38d5d4ae86a 28
IngmarLoohuis 6:6bc6ce1fe94e 29 //**************Go flags********************************************
IngmarLoohuis 6:6bc6ce1fe94e 30 volatile bool fn1_go = false;
IngmarLoohuis 6:6bc6ce1fe94e 31 void fn1_activate(){ fn1_go = true; }; //Activates the go−flag
IngmarLoohuis 7:742b1969f6c9 32 volatile bool fn2_go = false;
IngmarLoohuis 7:742b1969f6c9 33 void fn2_activate(){ fn2_go = true; }; //Activates the go-flag
IngmarLoohuis 7:742b1969f6c9 34 volatile bool fn3_go = false;
IngmarLoohuis 7:742b1969f6c9 35 void fn3_activate(){ fn3_go = true; }; //Activates the go-flag
IngmarLoohuis 15:d38d5d4ae86a 36 volatile bool fn4_go = false;
IngmarLoohuis 15:d38d5d4ae86a 37 void fn4_activate(){ fn4_go = true; }; //Activates the go−flag
IngmarLoohuis 15:d38d5d4ae86a 38 volatile bool fn5_go = false;
IngmarLoohuis 15:d38d5d4ae86a 39 void fn5_activate(){ fn5_go = true; }; //Activates the go-flag
IngmarLoohuis 15:d38d5d4ae86a 40 volatile bool fn6_go = false;
IngmarLoohuis 15:d38d5d4ae86a 41 void fn6_activate(){ fn6_go = true; }; //Activates the go-flag
IngmarLoohuis 6:6bc6ce1fe94e 42 //***************Global Variables***********************************
IngmarLoohuis 6:6bc6ce1fe94e 43 const double pi = 3.14159265359;
IngmarLoohuis 15:d38d5d4ae86a 44 //const double transmissionShoulder =94.4/40.2;
IngmarLoohuis 15:d38d5d4ae86a 45 //const double transmissionElbow = 1.0;
IngmarLoohuis 12:0cf4e70f6b8e 46 // controller constants
IngmarLoohuis 16:2083f634c91c 47 const double m1_Kp = 33760.9402682728, m1_Ki = 17183475.7181081, m1_Kd = 11.5703086491897, m1_N = 14221.4403045604;
IngmarLoohuis 15:d38d5d4ae86a 48 const double m2_Kp = 0.000048765659063912, m2_Ki = 0.0000228295351674407, m2_Kd = 0.0000255784613247063, m2_N = 54.5397025421619;
IngmarLoohuis 15:d38d5d4ae86a 49 const double m1_Ts = 0.001; // Controller sample time motor 1
IngmarLoohuis 15:d38d5d4ae86a 50 const double m2_Ts = 0.001; // Controller sample time motor 2
IngmarLoohuis 15:d38d5d4ae86a 51 double m1_v1 = 0;
IngmarLoohuis 15:d38d5d4ae86a 52 double m1_v2 = 0;
IngmarLoohuis 15:d38d5d4ae86a 53 double m2_v1 = 0;
IngmarLoohuis 15:d38d5d4ae86a 54 double m2_v2 = 0;
IngmarLoohuis 12:0cf4e70f6b8e 55 // position variable
IngmarLoohuis 15:d38d5d4ae86a 56 volatile double radians_m1;
IngmarLoohuis 15:d38d5d4ae86a 57 volatile double radians_m2;
IngmarLoohuis 16:2083f634c91c 58 volatile double error_m1;
IngmarLoohuis 16:2083f634c91c 59 volatile double error_m2;
IngmarLoohuis 16:2083f634c91c 60
IngmarLoohuis 12:0cf4e70f6b8e 61 //plant variable
IngmarLoohuis 9:6a065971d0ae 62 volatile double motor1;
IngmarLoohuis 15:d38d5d4ae86a 63 volatile double motor2;
IngmarLoohuis 15:d38d5d4ae86a 64
IngmarLoohuis 12:0cf4e70f6b8e 65
IngmarLoohuis 12:0cf4e70f6b8e 66
IngmarLoohuis 6:6bc6ce1fe94e 67 //*****************Angles Arms***********************
IngmarLoohuis 1:f26a53da33ed 68
IngmarLoohuis 6:6bc6ce1fe94e 69 double O1=1.7633;
IngmarLoohuis 6:6bc6ce1fe94e 70 double O2=2.0915;
IngmarLoohuis 6:6bc6ce1fe94e 71 double O3=1.8685;
IngmarLoohuis 6:6bc6ce1fe94e 72 double O4=1.1363;
IngmarLoohuis 6:6bc6ce1fe94e 73 double O5=2.3960;
IngmarLoohuis 6:6bc6ce1fe94e 74 double O6=2.0827;
IngmarLoohuis 6:6bc6ce1fe94e 75 double B1=1.3551;
IngmarLoohuis 6:6bc6ce1fe94e 76 double B2=0.5964;
IngmarLoohuis 6:6bc6ce1fe94e 77 double B3=0.06652;
IngmarLoohuis 6:6bc6ce1fe94e 78 double B4=0.0669;
IngmarLoohuis 6:6bc6ce1fe94e 79 double B5=1.7462;
IngmarLoohuis 6:6bc6ce1fe94e 80 double B6=-0.8994;
IngmarLoohuis 5:931594a366b7 81
IngmarLoohuis 6:6bc6ce1fe94e 82 //**********functions******************************
IngmarLoohuis 16:2083f634c91c 83 double PID1( double err, const double Kp, const double Ki, const double Kd,
IngmarLoohuis 12:0cf4e70f6b8e 84 const double Ts, const double N, double &v1, double &v2 ) {
IngmarLoohuis 12:0cf4e70f6b8e 85 // These variables are only calculated once!
IngmarLoohuis 12:0cf4e70f6b8e 86 const double a1 = (-4.0/(N*Ts+2));
IngmarLoohuis 12:0cf4e70f6b8e 87 const double a2 = -(N*Ts-2)/(N*Ts+2);
IngmarLoohuis 12:0cf4e70f6b8e 88 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 89 const double b1 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
IngmarLoohuis 12:0cf4e70f6b8e 90 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 91
IngmarLoohuis 12:0cf4e70f6b8e 92 double v = err - a1*v1 - a2*v2;
IngmarLoohuis 14:6ecf2b986a4b 93 double u = b0*v + b1*v1 + b2*v2;
IngmarLoohuis 12:0cf4e70f6b8e 94 v2 = v1; v1 = v;
IngmarLoohuis 12:0cf4e70f6b8e 95 return u;
IngmarLoohuis 12:0cf4e70f6b8e 96 }
IngmarLoohuis 12:0cf4e70f6b8e 97
IngmarLoohuis 16:2083f634c91c 98 double PID2( double err, const double Kp, const double Ki, const double Kd,
IngmarLoohuis 16:2083f634c91c 99 const double Ts, const double N, double &v1, double &v2 ) {
IngmarLoohuis 16:2083f634c91c 100 // These variables are only calculated once!
IngmarLoohuis 16:2083f634c91c 101 const double a1_2 = (-4.0/(N*Ts+2));
IngmarLoohuis 16:2083f634c91c 102 const double a2_2 = -(N*Ts-2)/(N*Ts+2);
IngmarLoohuis 16:2083f634c91c 103 const double b0_2 = (4.0*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
IngmarLoohuis 16:2083f634c91c 104 const double b1_2 = (Ki*N*pow(Ts,2) - 4.0*Kp - 4.0*Kd*N)/(N*Ts + 2.0);
IngmarLoohuis 16:2083f634c91c 105 const double b2_2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4);
IngmarLoohuis 16:2083f634c91c 106
IngmarLoohuis 16:2083f634c91c 107 double v_2 = err - a1_2*v1_2 - a2_2*v2_2;
IngmarLoohuis 16:2083f634c91c 108 double u_2 = b0_2*v_2 + b1_2*v1_2 + b2_2*v2_2;
IngmarLoohuis 16:2083f634c91c 109 v2_2 = v1_2; v1_2 = v_2;
IngmarLoohuis 16:2083f634c91c 110 return u_2;
IngmarLoohuis 16:2083f634c91c 111 }
IngmarLoohuis 9:6a065971d0ae 112
IngmarLoohuis 15:d38d5d4ae86a 113 void getAngPosition_m1() //Get angular position motor 1
IngmarLoohuis 6:6bc6ce1fe94e 114 {
IngmarLoohuis 15:d38d5d4ae86a 115 volatile int pulses_m1 = encoder_m1.getPulses();
IngmarLoohuis 15:d38d5d4ae86a 116 radians_m1 = (pulses_m1 / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
IngmarLoohuis 15:d38d5d4ae86a 117 }
IngmarLoohuis 13:f92e918af729 118
IngmarLoohuis 15:d38d5d4ae86a 119 void getAngPosition_m2() //Get angular position motor 2
IngmarLoohuis 15:d38d5d4ae86a 120 {
IngmarLoohuis 15:d38d5d4ae86a 121 volatile int pulses_m2 = encoder_m2.getPulses();
IngmarLoohuis 15:d38d5d4ae86a 122 radians_m2 = (pulses_m2 / (1 * 3591.84)) * 2*pi; //2 = encoding type, 3591.84 = counts per revoluton for the two big motors
IngmarLoohuis 6:6bc6ce1fe94e 123 }
IngmarLoohuis 6:6bc6ce1fe94e 124
IngmarLoohuis 7:742b1969f6c9 125 // Next task, measure the error and apply the output to the plant
IngmarLoohuis 15:d38d5d4ae86a 126 void motor1_Controller(double radians_m1)
IngmarLoohuis 7:742b1969f6c9 127 {
IngmarLoohuis 16:2083f634c91c 128 double reference_m1 = 2*pi;
IngmarLoohuis 15:d38d5d4ae86a 129 volatile double error_m1 = reference_m1 - radians_m1;
IngmarLoohuis 16:2083f634c91c 130 motor1 = PID1( error_m1,m1_Kp,m1_Ki,m1_Kd,m1_Ts, m1_N, m1_v1, m1_v2 );
IngmarLoohuis 15:d38d5d4ae86a 131 }
IngmarLoohuis 15:d38d5d4ae86a 132
IngmarLoohuis 15:d38d5d4ae86a 133 // Next task, measure the error and apply the output to the plant
IngmarLoohuis 15:d38d5d4ae86a 134 void motor2_Controller(double radians_m2)
IngmarLoohuis 15:d38d5d4ae86a 135 {
IngmarLoohuis 16:2083f634c91c 136 double reference_m2 = 2*pi;
IngmarLoohuis 15:d38d5d4ae86a 137 volatile double error_m2 = reference_m2 - radians_m2;
IngmarLoohuis 16:2083f634c91c 138 motor2 = PID2( error_m2,m2_Kp,m2_Ki,m2_Kd,m2_Ts, m2_N, m2_v1, m2_v2 );
IngmarLoohuis 15:d38d5d4ae86a 139
IngmarLoohuis 7:742b1969f6c9 140 }
IngmarLoohuis 7:742b1969f6c9 141
IngmarLoohuis 7:742b1969f6c9 142
IngmarLoohuis 15:d38d5d4ae86a 143
IngmarLoohuis 15:d38d5d4ae86a 144 void control_m1(double motor1)
IngmarLoohuis 4:30d8610b63a6 145 {
IngmarLoohuis 14:6ecf2b986a4b 146 if(abs(motor1)>0.000005)
IngmarLoohuis 2:665df4abd084 147 {
IngmarLoohuis 9:6a065971d0ae 148 motor1MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
IngmarLoohuis 2:665df4abd084 149 }
IngmarLoohuis 9:6a065971d0ae 150 else
IngmarLoohuis 2:665df4abd084 151 {
IngmarLoohuis 6:6bc6ce1fe94e 152 motor1MagnitudePin=0.0;
IngmarLoohuis 2:665df4abd084 153 }
IngmarLoohuis 9:6a065971d0ae 154 if(motor1<=0)
IngmarLoohuis 9:6a065971d0ae 155 {
IngmarLoohuis 9:6a065971d0ae 156 motor1DirectionPin=0.0;
IngmarLoohuis 9:6a065971d0ae 157 }
IngmarLoohuis 9:6a065971d0ae 158 else {
IngmarLoohuis 9:6a065971d0ae 159 motor1DirectionPin=1.0;
IngmarLoohuis 9:6a065971d0ae 160 }
IngmarLoohuis 0:2f40eb89ffce 161 }
IngmarLoohuis 7:742b1969f6c9 162
IngmarLoohuis 15:d38d5d4ae86a 163 void control_m2(double motor2)
IngmarLoohuis 15:d38d5d4ae86a 164 {
IngmarLoohuis 16:2083f634c91c 165 if(abs(motor2)>0.005)
IngmarLoohuis 15:d38d5d4ae86a 166 {
IngmarLoohuis 15:d38d5d4ae86a 167 motor2MagnitudePin=0.5;//MOET NOG TUSSENWAAREN KRIJGEN
IngmarLoohuis 15:d38d5d4ae86a 168 }
IngmarLoohuis 15:d38d5d4ae86a 169 else
IngmarLoohuis 15:d38d5d4ae86a 170 {
IngmarLoohuis 15:d38d5d4ae86a 171 motor2MagnitudePin=0.0;
IngmarLoohuis 15:d38d5d4ae86a 172 }
IngmarLoohuis 15:d38d5d4ae86a 173 if(motor2<=0)
IngmarLoohuis 15:d38d5d4ae86a 174 {
IngmarLoohuis 16:2083f634c91c 175 motor2DirectionPin=0.0;
IngmarLoohuis 15:d38d5d4ae86a 176 }
IngmarLoohuis 15:d38d5d4ae86a 177 else {
IngmarLoohuis 16:2083f634c91c 178 motor2DirectionPin=1.0;
IngmarLoohuis 15:d38d5d4ae86a 179 }
IngmarLoohuis 15:d38d5d4ae86a 180 }
IngmarLoohuis 15:d38d5d4ae86a 181
IngmarLoohuis 16:2083f634c91c 182 void print()
IngmarLoohuis 16:2083f634c91c 183 {
IngmarLoohuis 16:2083f634c91c 184 scope.set(0,radians_m1);
IngmarLoohuis 16:2083f634c91c 185 scope.send();
IngmarLoohuis 16:2083f634c91c 186 scope.set(1,radians_m2);
IngmarLoohuis 16:2083f634c91c 187 scope.send();
IngmarLoohuis 16:2083f634c91c 188 }
IngmarLoohuis 16:2083f634c91c 189
IngmarLoohuis 6:6bc6ce1fe94e 190 //****************MAIN FUNCTION*********************************
IngmarLoohuis 0:2f40eb89ffce 191 int main()
IngmarLoohuis 16:2083f634c91c 192 {HIDS.attach(print,0.001f);
IngmarLoohuis 15:d38d5d4ae86a 193 motor1MagnitudePin.period(1.0/1000.0);
IngmarLoohuis 15:d38d5d4ae86a 194 motor2MagnitudePin.period(1.0/1000.0);
IngmarLoohuis 13:f92e918af729 195 t1.attach(&fn1_activate, 0.0001f);
IngmarLoohuis 7:742b1969f6c9 196 t2.attach(&fn2_activate, 0.0001f);
IngmarLoohuis 7:742b1969f6c9 197 t3.attach(&fn3_activate, 0.0001f);
IngmarLoohuis 15:d38d5d4ae86a 198 t4.attach(&fn4_activate, 0.0001f);
IngmarLoohuis 15:d38d5d4ae86a 199 t5.attach(&fn5_activate, 0.0001f);
IngmarLoohuis 15:d38d5d4ae86a 200 t6.attach(&fn6_activate, 0.0001f);
IngmarLoohuis 11:eda4fbf91948 201 pc.baud(115200);
IngmarLoohuis 10:54b66bd1db20 202 while(true)
IngmarLoohuis 10:54b66bd1db20 203 {
IngmarLoohuis 6:6bc6ce1fe94e 204 if(fn1_go)
IngmarLoohuis 6:6bc6ce1fe94e 205 {
IngmarLoohuis 6:6bc6ce1fe94e 206 fn1_go = false;
IngmarLoohuis 15:d38d5d4ae86a 207 control_m1(motor1);
IngmarLoohuis 6:6bc6ce1fe94e 208 }
IngmarLoohuis 7:742b1969f6c9 209 if(fn2_go)
IngmarLoohuis 7:742b1969f6c9 210 {
IngmarLoohuis 7:742b1969f6c9 211 fn2_go = false;
IngmarLoohuis 15:d38d5d4ae86a 212 motor1_Controller(radians_m1);
IngmarLoohuis 7:742b1969f6c9 213 }
IngmarLoohuis 7:742b1969f6c9 214 if(fn3_go)
IngmarLoohuis 7:742b1969f6c9 215 {
IngmarLoohuis 7:742b1969f6c9 216 fn3_go = false;
IngmarLoohuis 15:d38d5d4ae86a 217 getAngPosition_m1();
IngmarLoohuis 15:d38d5d4ae86a 218 }
IngmarLoohuis 15:d38d5d4ae86a 219 if(fn4_go)
IngmarLoohuis 15:d38d5d4ae86a 220 {
IngmarLoohuis 15:d38d5d4ae86a 221 fn4_go = false;
IngmarLoohuis 15:d38d5d4ae86a 222 control_m2(motor2);
IngmarLoohuis 7:742b1969f6c9 223 }
IngmarLoohuis 15:d38d5d4ae86a 224 if(fn5_go)
IngmarLoohuis 15:d38d5d4ae86a 225 {
IngmarLoohuis 15:d38d5d4ae86a 226 fn5_go = false;
IngmarLoohuis 15:d38d5d4ae86a 227 motor2_Controller(radians_m2);
IngmarLoohuis 15:d38d5d4ae86a 228 }
IngmarLoohuis 15:d38d5d4ae86a 229 if(fn6_go)
IngmarLoohuis 15:d38d5d4ae86a 230 {
IngmarLoohuis 15:d38d5d4ae86a 231 fn6_go = false;
IngmarLoohuis 15:d38d5d4ae86a 232 getAngPosition_m2();
IngmarLoohuis 15:d38d5d4ae86a 233 }
IngmarLoohuis 15:d38d5d4ae86a 234
IngmarLoohuis 7:742b1969f6c9 235 }
IngmarLoohuis 2:665df4abd084 236 }