Biorobotics / Mbed 2 deprecated PID_Control

acos faalt

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@4:f0fd4a4ec178, 2015-10-19 (annotated)

Committer:
davevogel0
Date:
Mon Oct 19 18:53:54 2015 +0000
Revision:
4:f0fd4a4ec178
Parent:
3:337345f748cf
Child:
5:ec6dd614aa7e
Start to include servo.

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