Thijs Rakels / Mbed 2 deprecated NR_method

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

NR_method_1.cpp@0:0af507ea0d83, 2018-10-22 (annotated)

Committer:
Thijsjeee
Date:
Mon Oct 22 11:57:03 2018 +0000
Revision:
0:0af507ea0d83
Child:
1:fafea1d00d0c
NR method, PID controler, Carthesian Motion.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Thijsjeee 0:0af507ea0d83 1 #include "mbed.h"
Thijsjeee 0:0af507ea0d83 2 #include "BiQuad.h"
Thijsjeee 0:0af507ea0d83 3 #include <math.h>
Thijsjeee 0:0af507ea0d83 4 #include <stdio.h>
Thijsjeee 0:0af507ea0d83 5 #include <iostream>
Thijsjeee 0:0af507ea0d83 6 #include <stdlib.h>
Thijsjeee 0:0af507ea0d83 7 #include <ctime>
Thijsjeee 0:0af507ea0d83 8 #include <QEI.h>
Thijsjeee 0:0af507ea0d83 9
Thijsjeee 0:0af507ea0d83 10 //Define in/outputs
Thijsjeee 0:0af507ea0d83 11
Thijsjeee 0:0af507ea0d83 12 float counts = 8400;
Thijsjeee 0:0af507ea0d83 13
Thijsjeee 0:0af507ea0d83 14 DigitalOut Led(LED1);
Thijsjeee 0:0af507ea0d83 15 PwmOut PMW1(D5); // Motor 1
Thijsjeee 0:0af507ea0d83 16 DigitalOut M1(D4); // direction of motor 1
Thijsjeee 0:0af507ea0d83 17 PwmOut PMW2(D6); // Motor 2
Thijsjeee 0:0af507ea0d83 18 DigitalOut M2(D7); // direction of motor 2
Thijsjeee 0:0af507ea0d83 19
Thijsjeee 0:0af507ea0d83 20
Thijsjeee 0:0af507ea0d83 21
Thijsjeee 0:0af507ea0d83 22 //initializing Encoders
Thijsjeee 0:0af507ea0d83 23 QEI Enc1(D13,D12, NC , counts, QEI::X4_ENCODING); //Motor 1 encoder
Thijsjeee 0:0af507ea0d83 24 QEI Enc2(D11,D10, NC , counts, QEI::X4_ENCODING); // Motor 3 encoder this checks whetehter the motor has rotated
Thijsjeee 0:0af507ea0d83 25
Thijsjeee 0:0af507ea0d83 26 double Kp = 1;
Thijsjeee 0:0af507ea0d83 27 double Ki = 1;
Thijsjeee 0:0af507ea0d83 28 double Kd = 1;
Thijsjeee 0:0af507ea0d83 29 double Ts = 0.001;
Thijsjeee 0:0af507ea0d83 30
Thijsjeee 0:0af507ea0d83 31 float counts_a;
Thijsjeee 0:0af507ea0d83 32 float counts_b;
Thijsjeee 0:0af507ea0d83 33
Thijsjeee 0:0af507ea0d83 34
Thijsjeee 0:0af507ea0d83 35 //Define Variables
Thijsjeee 0:0af507ea0d83 36
Thijsjeee 0:0af507ea0d83 37 double pi = 3.14159265359;
Thijsjeee 0:0af507ea0d83 38 double bb;
Thijsjeee 0:0af507ea0d83 39 double bc;
Thijsjeee 0:0af507ea0d83 40 float z;
Thijsjeee 0:0af507ea0d83 41
Thijsjeee 0:0af507ea0d83 42 double angle_a = 0.5* pi; //in rad
Thijsjeee 0:0af507ea0d83 43 double angle_b = 0 * pi; //in rad
Thijsjeee 0:0af507ea0d83 44
Thijsjeee 0:0af507ea0d83 45 double X0[2][1] = {{angle_a},{angle_b}};
Thijsjeee 0:0af507ea0d83 46 double X[2][1];
Thijsjeee 0:0af507ea0d83 47 double Xold[2][1];
Thijsjeee 0:0af507ea0d83 48 double fval[2][1];
Thijsjeee 0:0af507ea0d83 49 double J[2][2];
Thijsjeee 0:0af507ea0d83 50 double err[2][1];
Thijsjeee 0:0af507ea0d83 51
Thijsjeee 0:0af507ea0d83 52 double MaxIter = 20;
Thijsjeee 0:0af507ea0d83 53 double tolX = 1e-4;
Thijsjeee 0:0af507ea0d83 54 double Loa = 10;
Thijsjeee 0:0af507ea0d83 55 double Lob = 10;
Thijsjeee 0:0af507ea0d83 56 double b = 10;
Thijsjeee 0:0af507ea0d83 57
Thijsjeee 0:0af507ea0d83 58 double c = 10;
Thijsjeee 0:0af507ea0d83 59 double Cx = 10.0; // current position
Thijsjeee 0:0af507ea0d83 60 double Cy = 10.0; // current position
Thijsjeee 0:0af507ea0d83 61 double Cxx = 12; // Goal position
Thijsjeee 0:0af507ea0d83 62 double Cyy = 12; // Goal position
Thijsjeee 0:0af507ea0d83 63
Thijsjeee 0:0af507ea0d83 64 Ticker position_controll;
Thijsjeee 0:0af507ea0d83 65
Thijsjeee 0:0af507ea0d83 66 void NR() //Newton Rapshon Calculation
Thijsjeee 0:0af507ea0d83 67 {
Thijsjeee 0:0af507ea0d83 68 //Variables
Thijsjeee 0:0af507ea0d83 69 double Hob = X[0][0];
Thijsjeee 0:0af507ea0d83 70 double Hoa = X[1][0];
Thijsjeee 0:0af507ea0d83 71
Thijsjeee 0:0af507ea0d83 72 //Define f(x)
Thijsjeee 0:0af507ea0d83 73 fval[0][0] = pow((Cx - Lob * sin(Hob)),2) + pow((Cy - Lob * cos(Hob)),2) - pow(c,2);
Thijsjeee 0:0af507ea0d83 74 fval[1][0] = pow((Cx - Loa * sin(Hoa)),2) + pow((Cy - Lob * cos(Hoa)),2) - pow(b,2);
Thijsjeee 0:0af507ea0d83 75 //Jacobian
Thijsjeee 0:0af507ea0d83 76
Thijsjeee 0:0af507ea0d83 77
Thijsjeee 0:0af507ea0d83 78 J[0][0]= -2 * Lob * cos(Hob) * (Cx - Lob * sin(Hob)) + 2 * Lob *sin(Hob) * (Cy - Lob * cos(Hob));
Thijsjeee 0:0af507ea0d83 79 J[1][1]= -2 * Loa * cos(Hoa) * (Cx - Loa * sin(Hoa)) + 2 * Loa* sin(Hoa) * (Cy - Loa * cos(Hoa));
Thijsjeee 0:0af507ea0d83 80 }
Thijsjeee 0:0af507ea0d83 81
Thijsjeee 0:0af507ea0d83 82
Thijsjeee 0:0af507ea0d83 83 void angle_define() //define the angle needed.
Thijsjeee 0:0af507ea0d83 84 {
Thijsjeee 0:0af507ea0d83 85 for(int i=1 ; i <= MaxIter; i++)
Thijsjeee 0:0af507ea0d83 86 {
Thijsjeee 0:0af507ea0d83 87 NR();
Thijsjeee 0:0af507ea0d83 88
Thijsjeee 0:0af507ea0d83 89 X[0][0] = X[0][0] - ((1/J[0][0]) * fval[0][0]);
Thijsjeee 0:0af507ea0d83 90 X[1][0] = X[1][0] - ((1/J[1][1]) * fval[1][0]);
Thijsjeee 0:0af507ea0d83 91
Thijsjeee 0:0af507ea0d83 92 err[0][0] = abs(X[0][0] - Xold[0][0]);
Thijsjeee 0:0af507ea0d83 93 err[1][0] = abs(X[1][0] - Xold[1][0]);
Thijsjeee 0:0af507ea0d83 94
Thijsjeee 0:0af507ea0d83 95 Xold[0][0] = X[0][0];
Thijsjeee 0:0af507ea0d83 96 Xold[1][0] = X[1][0];
Thijsjeee 0:0af507ea0d83 97
Thijsjeee 0:0af507ea0d83 98 counts_a = ((X[0][0]) / (2* pi)) * 8400;
Thijsjeee 0:0af507ea0d83 99 counts_b = -1 *(((X[1][0]) / (2* pi)) * 8400);
Thijsjeee 0:0af507ea0d83 100
Thijsjeee 0:0af507ea0d83 101 if(err[0][0] <= tolX)
Thijsjeee 0:0af507ea0d83 102 {
Thijsjeee 0:0af507ea0d83 103 if(err[1][0] <= tolX)
Thijsjeee 0:0af507ea0d83 104 {
Thijsjeee 0:0af507ea0d83 105 break;
Thijsjeee 0:0af507ea0d83 106 }
Thijsjeee 0:0af507ea0d83 107 }
Thijsjeee 0:0af507ea0d83 108 }
Thijsjeee 0:0af507ea0d83 109 }
Thijsjeee 0:0af507ea0d83 110
Thijsjeee 0:0af507ea0d83 111 void position_define()
Thijsjeee 0:0af507ea0d83 112 {
Thijsjeee 0:0af507ea0d83 113 if (Cx >= Cxx)
Thijsjeee 0:0af507ea0d83 114 {
Thijsjeee 0:0af507ea0d83 115 if (Cy >= Cyy)
Thijsjeee 0:0af507ea0d83 116 {
Thijsjeee 0:0af507ea0d83 117 }
Thijsjeee 0:0af507ea0d83 118 else
Thijsjeee 0:0af507ea0d83 119 {
Thijsjeee 0:0af507ea0d83 120 if (Cy > Cyy)
Thijsjeee 0:0af507ea0d83 121 {
Thijsjeee 0:0af507ea0d83 122 Cy = Cy - 0.005;
Thijsjeee 0:0af507ea0d83 123 }
Thijsjeee 0:0af507ea0d83 124 if (Cy < Cyy)
Thijsjeee 0:0af507ea0d83 125 {
Thijsjeee 0:0af507ea0d83 126 Cy = Cy + 0.005;
Thijsjeee 0:0af507ea0d83 127 }
Thijsjeee 0:0af507ea0d83 128 }
Thijsjeee 0:0af507ea0d83 129 }
Thijsjeee 0:0af507ea0d83 130 else
Thijsjeee 0:0af507ea0d83 131 {
Thijsjeee 0:0af507ea0d83 132 if (Cx > Cxx)
Thijsjeee 0:0af507ea0d83 133 {
Thijsjeee 0:0af507ea0d83 134 Cx = Cx - 0.005;
Thijsjeee 0:0af507ea0d83 135 }
Thijsjeee 0:0af507ea0d83 136 if (Cx < Cxx)
Thijsjeee 0:0af507ea0d83 137 {
Thijsjeee 0:0af507ea0d83 138 Cx = Cx + 0.005;
Thijsjeee 0:0af507ea0d83 139 }
Thijsjeee 0:0af507ea0d83 140 }
Thijsjeee 0:0af507ea0d83 141 }
Thijsjeee 0:0af507ea0d83 142
Thijsjeee 0:0af507ea0d83 143 double PID_controller(double error)
Thijsjeee 0:0af507ea0d83 144 {
Thijsjeee 0:0af507ea0d83 145 static double error_integral = 0;
Thijsjeee 0:0af507ea0d83 146 static double error_prev = error;
Thijsjeee 0:0af507ea0d83 147 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Thijsjeee 0:0af507ea0d83 148
Thijsjeee 0:0af507ea0d83 149 //proportional part
Thijsjeee 0:0af507ea0d83 150 double u_k = Kp * error;
Thijsjeee 0:0af507ea0d83 151
Thijsjeee 0:0af507ea0d83 152 //Integral part
Thijsjeee 0:0af507ea0d83 153 error_integral = error_integral + error * Ts;
Thijsjeee 0:0af507ea0d83 154 double u_i = Ki * error_integral;
Thijsjeee 0:0af507ea0d83 155
Thijsjeee 0:0af507ea0d83 156 // Derivative part
Thijsjeee 0:0af507ea0d83 157 double error_derivative = (error - error_prev)/Ts;
Thijsjeee 0:0af507ea0d83 158 double filtered_error_derivative = LowPassFilter.step(error_derivative);
Thijsjeee 0:0af507ea0d83 159 double u_d = Kd * filtered_error_derivative;
Thijsjeee 0:0af507ea0d83 160 error_prev = error;
Thijsjeee 0:0af507ea0d83 161 return ((u_k + u_i + u_d)/100);
Thijsjeee 0:0af507ea0d83 162 }
Thijsjeee 0:0af507ea0d83 163
Thijsjeee 0:0af507ea0d83 164 void motor_controler()
Thijsjeee 0:0af507ea0d83 165 {
Thijsjeee 0:0af507ea0d83 166 bb = Enc1.getPulses() + 2100;
Thijsjeee 0:0af507ea0d83 167 bc = Enc2.getPulses();
Thijsjeee 0:0af507ea0d83 168 if (bb >= counts_a)
Thijsjeee 0:0af507ea0d83 169 {
Thijsjeee 0:0af507ea0d83 170 z = PID_controller((counts_a - bb));
Thijsjeee 0:0af507ea0d83 171 PMW1.write(abs(z));
Thijsjeee 0:0af507ea0d83 172 M1 = 0;
Thijsjeee 0:0af507ea0d83 173 }
Thijsjeee 0:0af507ea0d83 174 if (bb <= counts_a)
Thijsjeee 0:0af507ea0d83 175 {
Thijsjeee 0:0af507ea0d83 176
Thijsjeee 0:0af507ea0d83 177 z = PID_controller((counts_a - bb));
Thijsjeee 0:0af507ea0d83 178 PMW1.write(abs(z));
Thijsjeee 0:0af507ea0d83 179 M1 = 1;
Thijsjeee 0:0af507ea0d83 180 }
Thijsjeee 0:0af507ea0d83 181 if (bc >= counts_b)
Thijsjeee 0:0af507ea0d83 182 {
Thijsjeee 0:0af507ea0d83 183 M2 = 0;
Thijsjeee 0:0af507ea0d83 184 z = PID_controller((counts_b) - bc);
Thijsjeee 0:0af507ea0d83 185 PMW2.write(abs(z));
Thijsjeee 0:0af507ea0d83 186 }
Thijsjeee 0:0af507ea0d83 187 if (bc <= counts_b)
Thijsjeee 0:0af507ea0d83 188 {
Thijsjeee 0:0af507ea0d83 189 M2 = 1;
Thijsjeee 0:0af507ea0d83 190 z = PID_controller((counts_b) - bc);
Thijsjeee 0:0af507ea0d83 191 PMW2.write(abs(z));
Thijsjeee 0:0af507ea0d83 192 }
Thijsjeee 0:0af507ea0d83 193 }
Thijsjeee 0:0af507ea0d83 194
Thijsjeee 0:0af507ea0d83 195 void position_controll_void()
Thijsjeee 0:0af507ea0d83 196 {
Thijsjeee 0:0af507ea0d83 197 Led = 1;
Thijsjeee 0:0af507ea0d83 198
Thijsjeee 0:0af507ea0d83 199 position_define();
Thijsjeee 0:0af507ea0d83 200 angle_define();
Thijsjeee 0:0af507ea0d83 201 motor_controler();
Thijsjeee 0:0af507ea0d83 202 Led = 0;
Thijsjeee 0:0af507ea0d83 203
Thijsjeee 0:0af507ea0d83 204
Thijsjeee 0:0af507ea0d83 205 }
Thijsjeee 0:0af507ea0d83 206
Thijsjeee 0:0af507ea0d83 207
Thijsjeee 0:0af507ea0d83 208 int main()
Thijsjeee 0:0af507ea0d83 209 {
Thijsjeee 0:0af507ea0d83 210 PMW1.period_us(60);
Thijsjeee 0:0af507ea0d83 211 X[0][0] = X0[0][0];
Thijsjeee 0:0af507ea0d83 212 X[1][0] = X0[1][0];
Thijsjeee 0:0af507ea0d83 213 Xold[0][0] = X0[0][0];
Thijsjeee 0:0af507ea0d83 214 Xold[1][0] = X0[1][0];
Thijsjeee 0:0af507ea0d83 215 position_controll.attach(position_controll_void,0.001);
Thijsjeee 0:0af507ea0d83 216 while(true)
Thijsjeee 0:0af507ea0d83 217 {
Thijsjeee 0:0af507ea0d83 218
Thijsjeee 0:0af507ea0d83 219
Thijsjeee 0:0af507ea0d83 220 }
Thijsjeee 0:0af507ea0d83 221 }