Biorobotics / Mbed 2 deprecated PID_Control

acos faalt

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@3:337345f748cf, 2015-10-19 (annotated)

Committer:
davevogel0
Date:
Mon Oct 19 10:31:01 2015 +0000
Revision:
3:337345f748cf
Parent:
2:da3b7dd2beb0
Child:
4:f0fd4a4ec178
Alexander v1;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
davevogel0 0:76bc19ed12ee 1 #include "mbed.h"
davevogel0 0:76bc19ed12ee 2 #include "HIDScope.h"
davevogel0 0:76bc19ed12ee 3 #include "MODSERIAL.h"
davevogel0 0:76bc19ed12ee 4 #include "QEI.h"
davevogel0 0:76bc19ed12ee 5
davevogel0 3:337345f748cf 6 //Aray of locations and Length of parts
davevogel0 3:337345f748cf 7 float X [10] = {-4, -3, -2, -1, 0, 1, 2, 3, 4, 5};
davevogel0 3:337345f748cf 8 float Y [10] = {-4, -3, -2, -1, 0, 1, 2, 3, 4, 5};
davevogel0 3:337345f748cf 9 const double L1 = 0.34;
davevogel0 3:337345f748cf 10 const double L2 = 0.26;
davevogel0 3:337345f748cf 11 const double R_Max_error = 0.01;
davevogel0 2:da3b7dd2beb0 12
davevogel0 0:76bc19ed12ee 13 //---------- Change motor control parameters-------
davevogel0 1:4465c9e203ce 14 //Motor 1
davevogel0 1:4465c9e203ce 15 const float M1_Ks = 0.4; //minimum power required to move
davevogel0 1:4465c9e203ce 16 const float M1_Kp = 0.1; //strentgh of proportional control
davevogel0 1:4465c9e203ce 17 const float M1_Kd = 0.1; //strentgh of differential control Include timestep in constant
davevogel0 1:4465c9e203ce 18 const float M1_Ki = 0.1; //strentgh of integrational control Include timestep in constant
davevogel0 1:4465c9e203ce 19 const double M1_friction = 0.55; //minimum power required to make the motor move
davevogel0 1:4465c9e203ce 20 //Motor 2
davevogel0 1:4465c9e203ce 21 const float M2_Ks = 0.4; //minimum power required to move
davevogel0 1:4465c9e203ce 22 const float M2_Kp = 0.1; //strentgh of proportional control
davevogel0 1:4465c9e203ce 23 const float M2_Kd = 0.1; //strentgh of differential control Include timestep in constant
davevogel0 1:4465c9e203ce 24 const float M2_Ki = 0.1; //strentgh of integrational control Include timestep in constant
davevogel0 1:4465c9e203ce 25 const double M2_friction = 0.55; //minimum power required to make the motor move
davevogel0 0:76bc19ed12ee 26 //----------End of control parameters
davevogel0 0:76bc19ed12ee 27
davevogel0 0:76bc19ed12ee 28 //encoder
davevogel0 0:76bc19ed12ee 29 QEI Motor1 (D13, D12, NC, 32);
davevogel0 0:76bc19ed12ee 30 QEI Motor2 (D11, D10, NC, 32);
davevogel0 0:76bc19ed12ee 31
davevogel0 0:76bc19ed12ee 32 //Define pins
davevogel0 0:76bc19ed12ee 33 DigitalOut M2(D7); //direction 2 //1 is cc 0=cw
davevogel0 0:76bc19ed12ee 34 PwmOut E2(D6); //speed 2
davevogel0 0:76bc19ed12ee 35 DigitalOut M1(D4); //direction 1 //1 is cc 0=cw
davevogel0 0:76bc19ed12ee 36 PwmOut E1(D5); //speed 1
davevogel0 0:76bc19ed12ee 37 AnalogIn pot1(A0); //read value of pot1 for position
davevogel0 0:76bc19ed12ee 38 AnalogIn pot2(A1); //read value of pot2 for position
davevogel0 0:76bc19ed12ee 39 DigitalOut myled(LED_GREEN);
davevogel0 0:76bc19ed12ee 40 MODSERIAL pc(USBTX, USBRX);
davevogel0 0:76bc19ed12ee 41 DigitalIn button(PTA4);
davevogel0 0:76bc19ed12ee 42 HIDScope scope(2);
davevogel0 0:76bc19ed12ee 43
davevogel0 0:76bc19ed12ee 44 //Define Variables
davevogel0 1:4465c9e203ce 45 double M1_dpos, M1_pos;
davevogel0 1:4465c9e203ce 46 double M2_dpos, M2_pos;
davevogel0 1:4465c9e203ce 47 double M_error, M1_error, M2_error;
davevogel0 3:337345f748cf 48 double R_set, A_R, R_error, R_error_int;
davevogel0 3:337345f748cf 49 double Q_set, Q_end, Q_error, Q_error_int;
davevogel0 1:4465c9e203ce 50 double position;
davevogel0 3:337345f748cf 51 const double long gearbox = 0.085877862;
davevogel0 2:da3b7dd2beb0 52 const double long PI = 3.14159265359;
davevogel0 1:4465c9e203ce 53 double M_error1 = 0;
davevogel0 0:76bc19ed12ee 54
davevogel0 0:76bc19ed12ee 55 //Timers and Tickers
davevogel0 0:76bc19ed12ee 56 Timer t;
davevogel0 0:76bc19ed12ee 57 Ticker t1,t2,t3;
davevogel0 0:76bc19ed12ee 58
davevogel0 0:76bc19ed12ee 59 //booleans run program
davevogel0 3:337345f748cf 60 volatile bool send_go = false, setpoint_go = false, control_go= false;
davevogel0 0:76bc19ed12ee 61
davevogel0 0:76bc19ed12ee 62 //------------------Activate programs-----------
davevogel0 0:76bc19ed12ee 63 //Activate send data pc
davevogel0 3:337345f748cf 64 void Send_True()
davevogel0 0:76bc19ed12ee 65 {
davevogel0 0:76bc19ed12ee 66 send_go = true;
davevogel0 0:76bc19ed12ee 67 }
davevogel0 0:76bc19ed12ee 68 // Activate desired location
davevogel0 1:4465c9e203ce 69 void M_Setpoint_True()
davevogel0 0:76bc19ed12ee 70 {
davevogel0 1:4465c9e203ce 71 setpoint_go = true;
davevogel0 1:4465c9e203ce 72 }
davevogel0 1:4465c9e203ce 73 // Controll if motor should go or not
davevogel0 1:4465c9e203ce 74 void M_Control_True()
davevogel0 1:4465c9e203ce 75 {
davevogel0 1:4465c9e203ce 76 control_go = true;
davevogel0 0:76bc19ed12ee 77 }
davevogel0 0:76bc19ed12ee 78 //------------------End of activate programs--------
davevogel0 0:76bc19ed12ee 79
davevogel0 0:76bc19ed12ee 80
davevogel0 0:76bc19ed12ee 81 //------------------Start of control programs-------
davevogel0 0:76bc19ed12ee 82
davevogel0 0:76bc19ed12ee 83 //Send values over HIDScope & Serial port
davevogel0 0:76bc19ed12ee 84 void Send()
davevogel0 0:76bc19ed12ee 85 {
davevogel0 0:76bc19ed12ee 86 scope.set(0,Motor1.getPulses());
davevogel0 0:76bc19ed12ee 87 scope.set(1,Motor2.getPulses());
davevogel0 0:76bc19ed12ee 88 scope.send();
davevogel0 1:4465c9e203ce 89 // pc.printf("Deg M1: %f M2: %f\n", M1_pos, M2_pos);
davevogel0 0:76bc19ed12ee 90 }
davevogel0 0:76bc19ed12ee 91
davevogel0 3:337345f748cf 92 //Desired Position Motors
davevogel0 1:4465c9e203ce 93 void Setpoint()
davevogel0 1:4465c9e203ce 94 {
davevogel0 3:337345f748cf 95 //set desired x and y point in array --> do this with EMG
davevogel0 2:da3b7dd2beb0 96 int dX = pot1.read() * 10;
davevogel0 2:da3b7dd2beb0 97 int dY = pot2.read() * 10;
davevogel0 2:da3b7dd2beb0 98
davevogel0 2:da3b7dd2beb0 99 //Set the length of the arm
davevogel0 3:337345f748cf 100 R_set = sqrt( X[dX] * X[dX] + Y[dY] * Y[dY]); //desired length of the arm
davevogel0 3:337345f748cf 101 Q_set = acos( Y[dY] / X[dX] ); //desired angle of the arm (theta for Qref) [rad]
davevogel0 1:4465c9e203ce 102 }
davevogel0 2:da3b7dd2beb0 103
davevogel0 3:337345f748cf 104 void Lengtharm()
davevogel0 2:da3b7dd2beb0 105 {
davevogel0 2:da3b7dd2beb0 106 //read out length of arm
davevogel0 3:337345f748cf 107 double A_Rx = L1 * cos(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * cos( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
davevogel0 3:337345f748cf 108 double A_Ry = L1 * sin(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * sin( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
davevogel0 3:337345f748cf 109 A_R = sqrt( pow (A_Rx, 2) + pow (A_Ry, 2)); //length Arm
davevogel0 3:337345f748cf 110 R_error= A_R-R_set; //error in arm length
davevogel0 2:da3b7dd2beb0 111
davevogel0 3:337345f748cf 112 pc.printf("R_set = %f A_R = %f R_error = %f \n", R_set, A_R, R_error);
davevogel0 2:da3b7dd2beb0 113 }
davevogel0 2:da3b7dd2beb0 114
davevogel0 3:337345f748cf 115 void Qpos()
davevogel0 0:76bc19ed12ee 116 {
davevogel0 3:337345f748cf 117 //read out pos of Motor 1
davevogel0 3:337345f748cf 118 double A_Rx = L1 * cos(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * cos( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
davevogel0 3:337345f748cf 119 double A_Ry = L1 * sin(gearbox * Motor1.getPulses() * PI / 180.0) + L2 * sin( (gearbox * Motor1.getPulses() + gearbox * Motor2.getPulses())* PI / 180.0);
davevogel0 3:337345f748cf 120 Q_end= acos( A_Ry / A_Rx ); //actual angle of the arm //Aandacht is dit graden(moet in radians)?!!!!!
davevogel0 3:337345f748cf 121 Q_error= Q_end-Q_set; //error in angle of Qref
davevogel0 3:337345f748cf 122
davevogel0 3:337345f748cf 123 pc.printf("Q_set = %f Q_end= %f Q_error = %f \n", Q_set, Q_end, Q_error);
davevogel0 3:337345f748cf 124 }
davevogel0 3:337345f748cf 125
davevogel0 3:337345f748cf 126 double Pw_control (double S_error, double& S_error_int, double Ks, double Kp,double Ki,double Kd, double friction)
davevogel0 3:337345f748cf 127 {
davevogel0 1:4465c9e203ce 128 // Motor Power
davevogel0 3:337345f748cf 129 M_error = S_error; //give the amount of error
davevogel0 3:337345f748cf 130
davevogel0 3:337345f748cf 131 double M_error_int = S_error_int + M_error / 1e4;
davevogel0 3:337345f748cf 132 S_error_int = M_error_int;
davevogel0 1:4465c9e203ce 133 double Ps = Ks;
davevogel0 1:4465c9e203ce 134 double Pp = Kp * M_error; //Proportional control
davevogel0 3:337345f748cf 135 double Pi = Ki * M_error_int; //check dit --> nog niet goed //int controll
davevogel0 3:337345f748cf 136 double Pd = Kd * (M_error - M_error1); //Differtial control
davevogel0 1:4465c9e203ce 137 M_error1 = M_error;
davevogel0 1:4465c9e203ce 138 double Power = Ps + Pp + Pi + Pd;
davevogel0 1:4465c9e203ce 139 pc.printf("Power bf: %f Pi: %f \n", Power, Pi);
davevogel0 1:4465c9e203ce 140
davevogel0 3:337345f748cf 141 // overcome minimum power required to turn and stop the motor from 'piepen' Also limit power to a max.
davevogel0 1:4465c9e203ce 142 if (Power<friction) {
davevogel0 1:4465c9e203ce 143 Power=0;
davevogel0 3:337345f748cf 144 } else if (Power>0.7) {
davevogel0 3:337345f748cf 145 Power=0.7;
davevogel0 1:4465c9e203ce 146 } else {}
davevogel0 1:4465c9e203ce 147
davevogel0 1:4465c9e203ce 148 //print error
davevogel0 1:4465c9e203ce 149 // pc.printf("Error: %f Direction: %f Position M: %f Setpoint %f \n", M_error, position, setpoint);
davevogel0 1:4465c9e203ce 150
davevogel0 1:4465c9e203ce 151 pc.printf("Power: %f\n", Power);
davevogel0 1:4465c9e203ce 152 return Power ;//is it possible to return 2 things for
davevogel0 1:4465c9e203ce 153 }
davevogel0 3:337345f748cf 154
davevogel0 1:4465c9e203ce 155 // direction controll
davevogel0 3:337345f748cf 156 bool dr_control (double A, double B)
davevogel0 1:4465c9e203ce 157 {
davevogel0 1:4465c9e203ce 158 // Direction motor should turn
davevogel0 3:337345f748cf 159 int Direction = (A > B) ? false:true;
davevogel0 1:4465c9e203ce 160 return Direction;
davevogel0 1:4465c9e203ce 161 }
davevogel0 3:337345f748cf 162
davevogel0 1:4465c9e203ce 163 // begin to set point
davevogel0 1:4465c9e203ce 164 void calibration()
davevogel0 1:4465c9e203ce 165 {
davevogel0 2:da3b7dd2beb0 166 M1_dpos = 0;
davevogel0 2:da3b7dd2beb0 167 M2_dpos = 0;
davevogel0 2:da3b7dd2beb0 168 if (button == true) {
davevogel0 2:da3b7dd2beb0 169 M1_pos = 0;
davevogel0 2:da3b7dd2beb0 170 M2_pos = 0;
davevogel0 1:4465c9e203ce 171 } else {
davevogel0 1:4465c9e203ce 172 }
davevogel0 0:76bc19ed12ee 173 }
davevogel0 0:76bc19ed12ee 174
davevogel0 0:76bc19ed12ee 175
davevogel0 0:76bc19ed12ee 176 //--------------- End of control programs----------
davevogel0 0:76bc19ed12ee 177 int main()
davevogel0 0:76bc19ed12ee 178 {
davevogel0 0:76bc19ed12ee 179 //turn that led off!It hurts my eyes! Ow, I do boot.
davevogel0 2:da3b7dd2beb0 180 myled = 1;
davevogel0 0:76bc19ed12ee 181
davevogel0 0:76bc19ed12ee 182 //PWM period motors
davevogel0 0:76bc19ed12ee 183 E1.period(0.0001f);
davevogel0 0:76bc19ed12ee 184 E2.period(0.0001f);
davevogel0 0:76bc19ed12ee 185
davevogel0 0:76bc19ed12ee 186 pc.baud(115200);
davevogel0 0:76bc19ed12ee 187
davevogel0 0:76bc19ed12ee 188 //sub programs - time how fast everything occurs
davevogel0 3:337345f748cf 189 t1.attach_us(&Send_True, 1e4); //Send data to pc
davevogel0 1:4465c9e203ce 190 t2.attach_us(&M_Setpoint_True, 1e4); //Desired position motor(EMG goes here)
davevogel0 1:4465c9e203ce 191 t3.attach_us(M_Control_True, 1e4); //Speed control here
davevogel0 0:76bc19ed12ee 192
davevogel0 3:337345f748cf 193 //calibration
davevogel0 0:76bc19ed12ee 194
davevogel0 3:337345f748cf 195
davevogel0 3:337345f748cf 196 //-------------Schedule programs-------------------
davevogel0 0:76bc19ed12ee 197 while(1) {
davevogel0 1:4465c9e203ce 198 if (setpoint_go == true) {
davevogel0 3:337345f748cf 199 if (run ==true) {
davevogel0 3:337345f748cf 200 Setpoint();
davevogel0 3:337345f748cf 201 } else if (run == false) {
davevogel0 3:337345f748cf 202 calibration();
davevogel0 3:337345f748cf 203 } else {}
davevogel0 1:4465c9e203ce 204 setpoint_go = false;
davevogel0 3:337345f748cf 205 } else if (control_go == true) {
davevogel0 3:337345f748cf 206 Lengtharm();
davevogel0 3:337345f748cf 207 Qpos();
davevogel0 3:337345f748cf 208 if (-1*R_Max_error<R_error<R_Max_error) {
davevogel0 3:337345f748cf 209 // control Motor2
davevogel0 3:337345f748cf 210 E2 = Pw_control (R_error, R_error_int, M2_Ks, M2_Kp, M2_Ki, M2_Kd, M2_friction);
davevogel0 3:337345f748cf 211 M2 = dr_control (A_R,R_set);
davevogel0 3:337345f748cf 212 M2_error = M_error;
davevogel0 3:337345f748cf 213 //print error
davevogel0 3:337345f748cf 214 pc.printf("M1_pos = %f M1_error = %f M2_pos = %f M2_error = %f\n", M1_pos, M1_error, M2_pos, M2_error);
davevogel0 3:337345f748cf 215 } // once this works place it in 'send'
davevogel0 3:337345f748cf 216 else if (-1*R_Max_error>R_error>R_Max_error) {
davevogel0 3:337345f748cf 217 // control Motor 1
davevogel0 3:337345f748cf 218 E1 = Pw_control (Q_error, Q_error_int, M1_Ks, M1_Kp, M1_Ki, M1_Kd, M1_friction);
davevogel0 3:337345f748cf 219 M1 = dr_control (Q_end, Q_set);
davevogel0 3:337345f748cf 220 // control Motor 2 - has to do nothing
davevogel0 3:337345f748cf 221 E2 = 0;
davevogel0 3:337345f748cf 222 } else {}
davevogel0 1:4465c9e203ce 223 control_go = false;
davevogel0 2:da3b7dd2beb0 224 } else if (send_go == true) {
davevogel0 0:76bc19ed12ee 225 Send();
davevogel0 0:76bc19ed12ee 226
davevogel0 0:76bc19ed12ee 227 send_go = false;
davevogel0 0:76bc19ed12ee 228 } else {
davevogel0 0:76bc19ed12ee 229
davevogel0 0:76bc19ed12ee 230 //-----------End of scedule progrmas----------------
davevogel0 0:76bc19ed12ee 231 }
davevogel0 0:76bc19ed12ee 232 }
davevogel0 0:76bc19ed12ee 233 }