group14 - k9 / Mbed 2 deprecated testPID

testcode pid

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of testPID by Martijn Kern

main.cpp@33:5477deb3803e, 2015-10-21 (annotated)

Committer:
Vigilance88
Date:
Wed Oct 21 12:17:04 2015 +0000
Revision:
33:5477deb3803e
Parent:
32:054900bfb0a5 
zou moeten werken..

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
Vigilance88 18:44905b008f44 3 #include "MODSERIAL.h"
Vigilance88 18:44905b008f44 4 #include "biquadFilter.h"
Vigilance88 18:44905b008f44 5 #include "QEI.h"
Vigilance88 21:d6a46315d5f5 6 #include "math.h"
Vigilance88 26:fe3a5469dd6b 7 #include <string>
Vigilance88 21:d6a46315d5f5 8
Vigilance88 21:d6a46315d5f5 9 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 10 -------------------------------- BIOROBOTICS GROUP 14 ----------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 11 --------------------------------------------------------------------------------------------------------------------*/
vsluiter 0:32bb76391d89 12
Vigilance88 18:44905b008f44 13 //Define important constants in memory
Vigilance88 21:d6a46315d5f5 14 #define PI 3.14159265
Vigilance88 18:44905b008f44 15 #define SAMPLE_RATE 0.002 //500 Hz EMG sample rate
Vigilance88 18:44905b008f44 16 #define CONTROL_RATE 0.01 //100 Hz Control rate
Vigilance88 21:d6a46315d5f5 17 #define ENCODER1_CPR 4200 //encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
Vigilance88 21:d6a46315d5f5 18 #define ENCODER2_CPR 4200 //gearbox 1:131.25 -> 4200 counts per revolution of the output shaft (X2),
Vigilance88 26:fe3a5469dd6b 19 #define PWM_PERIOD 0.0001 //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly
Vigilance88 21:d6a46315d5f5 20 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 21 ---- OBJECTS ---------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 22 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 23
Vigilance88 18:44905b008f44 24 MODSERIAL pc(USBTX,USBRX); //serial communication
Vigilance88 18:44905b008f44 25
Vigilance88 33:5477deb3803e 26 //Debug LEDs
Vigilance88 25:49ccdc98639a 27 DigitalOut red(LED_RED);
Vigilance88 25:49ccdc98639a 28 DigitalOut green(LED_GREEN);
Vigilance88 25:49ccdc98639a 29 DigitalOut blue(LED_BLUE);
Vigilance88 25:49ccdc98639a 30
Vigilance88 18:44905b008f44 31
Vigilance88 18:44905b008f44 32 Ticker sample_timer; //Ticker for EMG sampling
Vigilance88 18:44905b008f44 33 Ticker control_timer; //Ticker for control loop
Vigilance88 18:44905b008f44 34 HIDScope scope(4); //Scope 4 channels
Vigilance88 18:44905b008f44 35
Vigilance88 18:44905b008f44 36 // AnalogIn potmeter(A4); //potmeters
Vigilance88 18:44905b008f44 37 // AnalogIn potmeter2(A5); //
Vigilance88 18:44905b008f44 38
Vigilance88 21:d6a46315d5f5 39 //Encoders
Vigilance88 18:44905b008f44 40 QEI Encoder1(D13,D12,NC,32); //channel A and B from encoder, counts = Encoder.getPulses();
Vigilance88 18:44905b008f44 41 QEI Encoder2(D10,D9,NC,32); //channel A and B from encoder,
Vigilance88 21:d6a46315d5f5 42
Vigilance88 21:d6a46315d5f5 43 //Speed and Direction of motors - D4 (dir) and D5(speed) = motor 2, D7(dir) and D6(speed) = motor 1
Vigilance88 21:d6a46315d5f5 44 PwmOut pwm_motor1(D6); //PWM motor 1
Vigilance88 21:d6a46315d5f5 45 PwmOut pwm_motor2(D5); //PWM motor 2
Vigilance88 26:fe3a5469dd6b 46
Vigilance88 18:44905b008f44 47 DigitalOut dir_motor1(D7); //Direction motor 1
Vigilance88 18:44905b008f44 48 DigitalOut dir_motor2(D4); //Direction motor 2
Vigilance88 18:44905b008f44 49
Vigilance88 24:56db31267f10 50 //Limit Switches
Vigilance88 27:0cefe83f83d3 51 InterruptIn shoulder_limit(D2); //using FRDM buttons
Vigilance88 27:0cefe83f83d3 52 InterruptIn elbow_limit(D3); //using FRDM buttons
Vigilance88 26:fe3a5469dd6b 53
Vigilance88 26:fe3a5469dd6b 54 //Other buttons
Vigilance88 26:fe3a5469dd6b 55 DigitalIn button1(PTA4); //using FRDM buttons
Vigilance88 26:fe3a5469dd6b 56 DigitalIn button2(PTC6); //using FRDM buttons
Vigilance88 26:fe3a5469dd6b 57
Vigilance88 24:56db31267f10 58
Vigilance88 21:d6a46315d5f5 59
Vigilance88 21:d6a46315d5f5 60 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 61 ---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 62 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 63
Vigilance88 25:49ccdc98639a 64
Vigilance88 24:56db31267f10 65 //PID variables
Vigilance88 24:56db31267f10 66 double u1; double u2; //Output of PID controller (PWM value for motor 1 and 2)
Vigilance88 24:56db31267f10 67 double m1_error=0; double m1_e_int=0; double m1_e_prev=0; //Error, integrated error, previous error
Vigilance88 32:054900bfb0a5 68 const double m1_kp=0.01; const double m1_ki=0.00125; const double m1_kd=0.00; //Proportional, integral and derivative gains.
Vigilance88 24:56db31267f10 69
Vigilance88 26:fe3a5469dd6b 70 double m2_error=0; double m2_e_int=0; double m2_e_prev=0; //Error, integrated error, previous error
Vigilance88 32:054900bfb0a5 71 const double m2_kp=0.01; const double m2_ki=0.00125; const double m2_kd=0.00; //Proportional, integral and derivative gains.
Vigilance88 24:56db31267f10 72
Vigilance88 24:56db31267f10 73
Vigilance88 26:fe3a5469dd6b 74 //lowpass filter 7 Hz - envelope
Vigilance88 24:56db31267f10 75 const double low_b0 = 0.000119046743110057;
Vigilance88 24:56db31267f10 76 const double low_b1 = 0.000238093486220118;
Vigilance88 24:56db31267f10 77 const double low_b2 = 0.000119046743110057;
Vigilance88 24:56db31267f10 78 const double low_a1 = -1.968902268531908;
Vigilance88 24:56db31267f10 79 const double low_a2 = 0.9693784555043481;
Vigilance88 21:d6a46315d5f5 80
Vigilance88 27:0cefe83f83d3 81 //Forward Kinematics
Vigilance88 27:0cefe83f83d3 82 const double l1 = 0.25; const double l2 = 0.25;
Vigilance88 27:0cefe83f83d3 83 double current_x; double current_y;
Vigilance88 27:0cefe83f83d3 84 double theta1; double theta2;
Vigilance88 27:0cefe83f83d3 85 double dtheta1; double dtheta2;
Vigilance88 27:0cefe83f83d3 86 double rpc;
Vigilance88 27:0cefe83f83d3 87 double x=0.5; double y=0;
Vigilance88 27:0cefe83f83d3 88 double x_error; double y_error;
Vigilance88 27:0cefe83f83d3 89 double costheta1; double sintheta1;
Vigilance88 27:0cefe83f83d3 90 double costheta2; double sintheta2;
Vigilance88 21:d6a46315d5f5 91 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 24:56db31267f10 92 ---- Filters ---------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 93 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 24:56db31267f10 94
Vigilance88 24:56db31267f10 95 //Using biquadFilter library
Vigilance88 24:56db31267f10 96 //Syntax: biquadFilter filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered.
Vigilance88 25:49ccdc98639a 97
Vigilance88 26:fe3a5469dd6b 98 //PID filter (lowpass ??? Hz)
Vigilance88 27:0cefe83f83d3 99 biquadFilter derfilter( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // derivative filter
Vigilance88 30:ec66691d226d 100 biquadFilter derfilter2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );
Vigilance88 24:56db31267f10 101 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 24:56db31267f10 102 ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------
Vigilance88 24:56db31267f10 103 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 26:fe3a5469dd6b 104
Vigilance88 27:0cefe83f83d3 105
Vigilance88 26:fe3a5469dd6b 106 void control(); //Control - reference -> error -> pid -> motor output
Vigilance88 27:0cefe83f83d3 107
Vigilance88 27:0cefe83f83d3 108
Vigilance88 26:fe3a5469dd6b 109 void calibrate_arm(void); //Calibration of the arm with limit switches
Vigilance88 26:fe3a5469dd6b 110 void start_sampling(void); //Attaches the sample_filter function to a 500Hz ticker
Vigilance88 26:fe3a5469dd6b 111 void stop_sampling(void); //Stops sample_filter
Vigilance88 26:fe3a5469dd6b 112 void start_control(void); //Attaches the control function to a 100Hz ticker
Vigilance88 26:fe3a5469dd6b 113 void stop_control(void); //Stops control function
Vigilance88 26:fe3a5469dd6b 114
Vigilance88 26:fe3a5469dd6b 115 //Keeps the input between min and max value
Vigilance88 24:56db31267f10 116 void keep_in_range(double * in, double min, double max);
Vigilance88 26:fe3a5469dd6b 117
Vigilance88 26:fe3a5469dd6b 118 //Reusable PID controller, previous and integral error need to be passed by reference
Vigilance88 21:d6a46315d5f5 119 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
Vigilance88 30:ec66691d226d 120 double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
Vigilance88 18:44905b008f44 121
Vigilance88 26:fe3a5469dd6b 122 //Menu functions
Vigilance88 21:d6a46315d5f5 123 void mainMenu();
Vigilance88 21:d6a46315d5f5 124 void caliMenu();
Vigilance88 26:fe3a5469dd6b 125 void clearTerminal();
Vigilance88 27:0cefe83f83d3 126 void controlMenu();
Vigilance88 21:d6a46315d5f5 127
Vigilance88 21:d6a46315d5f5 128 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 129 ---- MAIN LOOP -------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 130 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 131
Vigilance88 30:ec66691d226d 132 volatile bool looptimerflag;
Vigilance88 30:ec66691d226d 133
Vigilance88 30:ec66691d226d 134 void setlooptimerflag(void)
Vigilance88 30:ec66691d226d 135 {
Vigilance88 30:ec66691d226d 136 looptimerflag = true;
Vigilance88 30:ec66691d226d 137 }
Vigilance88 30:ec66691d226d 138
Vigilance88 21:d6a46315d5f5 139 int main()
Vigilance88 21:d6a46315d5f5 140 {
Vigilance88 26:fe3a5469dd6b 141 pc.baud(115200); //terminal baudrate
Vigilance88 26:fe3a5469dd6b 142 red=1; green=1; blue=1; //Make sure debug LEDS are off
Vigilance88 26:fe3a5469dd6b 143
Vigilance88 27:0cefe83f83d3 144 //Set PwmOut frequency to 10k Hz???
Vigilance88 30:ec66691d226d 145 pwm_motor1.period(0.0001);
Vigilance88 30:ec66691d226d 146 pwm_motor2.period(0.0001);
Vigilance88 27:0cefe83f83d3 147
Vigilance88 26:fe3a5469dd6b 148
Vigilance88 30:ec66691d226d 149 //VARIABLES
Vigilance88 30:ec66691d226d 150 AnalogIn potmeter(A4);
Vigilance88 30:ec66691d226d 151 AnalogIn potmeter2(A5);
Vigilance88 30:ec66691d226d 152 //DigitalIn button(D8);
Vigilance88 30:ec66691d226d 153 //MODSERIAL pc(USBTX,USBRX);
Vigilance88 26:fe3a5469dd6b 154
Vigilance88 30:ec66691d226d 155 //Encoder motor1(D13,D12); // channel A and B from encoder, true - getSpeed works
Vigilance88 30:ec66691d226d 156 //PwmOut pwm_motor1(D6); // D4 and D5 = motor 2, D7 and D6 = motor 2
Vigilance88 30:ec66691d226d 157 //DigitalOut dir_motor1(D7); //
Vigilance88 25:49ccdc98639a 158
Vigilance88 30:ec66691d226d 159 //Encoder motor2(D10,D9); // channel A and B from encoder, true - getSpeed works
Vigilance88 30:ec66691d226d 160 // PwmOut pwm_motor2(D5); // D4 and D5 = motor 2, D7 and D6 = motor 2
Vigilance88 30:ec66691d226d 161 // DigitalOut dir_motor2(D4); //
Vigilance88 21:d6a46315d5f5 162
Vigilance88 30:ec66691d226d 163 // MOTOR1
Vigilance88 30:ec66691d226d 164 double goal;
Vigilance88 30:ec66691d226d 165 double pwm_to_motor;
Vigilance88 30:ec66691d226d 166 // MOTOR2
Vigilance88 30:ec66691d226d 167 double goal2;
Vigilance88 30:ec66691d226d 168 double pwm_to_motor2;
Vigilance88 30:ec66691d226d 169
Vigilance88 30:ec66691d226d 170 //CODE
Vigilance88 30:ec66691d226d 171 pc.baud(115200);
Vigilance88 27:0cefe83f83d3 172
Vigilance88 30:ec66691d226d 173 //pwm_motor1.write(0.2f); // Speed
Vigilance88 30:ec66691d226d 174 //dir_motor1.write(1); // Direction
Vigilance88 30:ec66691d226d 175
Vigilance88 30:ec66691d226d 176 Ticker looptimer;
Vigilance88 30:ec66691d226d 177 looptimer.attach(setlooptimerflag,0.01);
Vigilance88 30:ec66691d226d 178
Vigilance88 30:ec66691d226d 179 while (1) {
Vigilance88 30:ec66691d226d 180
Vigilance88 30:ec66691d226d 181 while(looptimerflag != true);
Vigilance88 30:ec66691d226d 182 looptimerflag = false;
Vigilance88 30:ec66691d226d 183
Vigilance88 30:ec66691d226d 184 //MOTOR1
Vigilance88 30:ec66691d226d 185 goal = (potmeter.read()-0.5)*4200;
Vigilance88 30:ec66691d226d 186 //pc.printf("setpoint: %f, %d, %f \n\r", goal, motor1.getPosition(),motor1.getSpeed());
Vigilance88 30:ec66691d226d 187 double error1 = (goal - Encoder1.getPulses());
Vigilance88 30:ec66691d226d 188 pwm_to_motor = pid(error1,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev);
Vigilance88 30:ec66691d226d 189
Vigilance88 30:ec66691d226d 190 if(pwm_to_motor > 0)
Vigilance88 30:ec66691d226d 191 dir_motor1 = 1; //=clockwise
Vigilance88 30:ec66691d226d 192 else
Vigilance88 30:ec66691d226d 193 dir_motor1 = 0; //=counterclockwise
Vigilance88 30:ec66691d226d 194
Vigilance88 30:ec66691d226d 195 pwm_motor1.write(abs(pwm_to_motor));
Vigilance88 30:ec66691d226d 196
Vigilance88 30:ec66691d226d 197 //MOTOR2
Vigilance88 30:ec66691d226d 198 goal2 = (potmeter2.read()-0.5)*4200;
Vigilance88 30:ec66691d226d 199
Vigilance88 30:ec66691d226d 200 double error2=(goal2 - Encoder2.getPulses());
Vigilance88 32:054900bfb0a5 201 pwm_to_motor2 = pid(error2,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev);
Vigilance88 30:ec66691d226d 202
Vigilance88 30:ec66691d226d 203 if(pwm_to_motor2 > 0)
Vigilance88 30:ec66691d226d 204 dir_motor2 = 0; //=counterclockwise
Vigilance88 30:ec66691d226d 205 else
Vigilance88 30:ec66691d226d 206 dir_motor2 = 1; //=clockwise
Vigilance88 30:ec66691d226d 207
Vigilance88 30:ec66691d226d 208 pwm_motor2.write(abs(pwm_to_motor2));
Vigilance88 30:ec66691d226d 209 //pc.printf("setpoint: %f, %d, %f \n\r", goal2, motor2.getPosition());
Vigilance88 30:ec66691d226d 210
Vigilance88 30:ec66691d226d 211
Vigilance88 30:ec66691d226d 212 pc.printf("goal: %f, pulses: %d \n\r", goal, Encoder1.getPulses());
Vigilance88 30:ec66691d226d 213 pc.printf("goal2: %f, pulses2: %d \n\r", goal2, Encoder2.getPulses());
Vigilance88 30:ec66691d226d 214
Vigilance88 21:d6a46315d5f5 215 }
Vigilance88 21:d6a46315d5f5 216 //end of while loop
Vigilance88 21:d6a46315d5f5 217 }
Vigilance88 21:d6a46315d5f5 218 //end of main
Vigilance88 21:d6a46315d5f5 219
Vigilance88 21:d6a46315d5f5 220 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 221 ---- FUNCTIONS -------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 222 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 18:44905b008f44 223
Vigilance88 27:0cefe83f83d3 224
Vigilance88 18:44905b008f44 225 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
Vigilance88 19:0a3ee31dcdb4 226 void calibrate_arm(void)
Vigilance88 19:0a3ee31dcdb4 227 {
Vigilance88 27:0cefe83f83d3 228 pc.printf("Calibrate_arm() entered\r\n");
Vigilance88 26:fe3a5469dd6b 229 bool calibrating = true;
Vigilance88 26:fe3a5469dd6b 230 bool done1 = false;
Vigilance88 26:fe3a5469dd6b 231 bool done2 = false;
Vigilance88 27:0cefe83f83d3 232 pc.printf("To start arm calibration, press any key =>");
Vigilance88 27:0cefe83f83d3 233 pc.getc();
Vigilance88 27:0cefe83f83d3 234 pc.printf("\r\n Calibrating... \r\n");
Vigilance88 26:fe3a5469dd6b 235 dir_motor1=1; //cw
Vigilance88 27:0cefe83f83d3 236 dir_motor2=0; //cw
Vigilance88 27:0cefe83f83d3 237 pwm_motor1.write(0.2); //move upper arm slowly cw
Vigilance88 27:0cefe83f83d3 238
Vigilance88 27:0cefe83f83d3 239
Vigilance88 26:fe3a5469dd6b 240
Vigilance88 26:fe3a5469dd6b 241 while(calibrating){
Vigilance88 27:0cefe83f83d3 242 red=0; blue=0; //Debug light is purple during arm calibration
Vigilance88 27:0cefe83f83d3 243
Vigilance88 27:0cefe83f83d3 244 if(done1==true){
Vigilance88 27:0cefe83f83d3 245 pwm_motor2.write(0.2); //move forearm slowly cw
Vigilance88 27:0cefe83f83d3 246 }
Vigilance88 27:0cefe83f83d3 247
Vigilance88 26:fe3a5469dd6b 248 //when limit switches are hit, stop motor and reset encoder
Vigilance88 26:fe3a5469dd6b 249 if(shoulder_limit.read() < 0.5){ //polling
Vigilance88 26:fe3a5469dd6b 250 pwm_motor1.write(0);
Vigilance88 26:fe3a5469dd6b 251 Encoder1.reset();
Vigilance88 26:fe3a5469dd6b 252 done1 = true;
Vigilance88 27:0cefe83f83d3 253 pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
Vigilance88 26:fe3a5469dd6b 254 }
Vigilance88 26:fe3a5469dd6b 255 if(elbow_limit.read() < 0.5){ //polling
Vigilance88 26:fe3a5469dd6b 256 pwm_motor2.write(0);
Vigilance88 26:fe3a5469dd6b 257 Encoder2.reset();
Vigilance88 26:fe3a5469dd6b 258 done2 = true;
Vigilance88 27:0cefe83f83d3 259 pc.printf("Elbow Limit hit - shutting down motor 2. \r\n");
Vigilance88 26:fe3a5469dd6b 260 }
Vigilance88 26:fe3a5469dd6b 261 if(done1 && done2){
Vigilance88 26:fe3a5469dd6b 262 calibrating=false; //Leave while loop when both limits are reached
Vigilance88 26:fe3a5469dd6b 263 red=1; blue=1; //Turn debug light off when calibration complete
Vigilance88 26:fe3a5469dd6b 264 }
Vigilance88 27:0cefe83f83d3 265
Vigilance88 26:fe3a5469dd6b 266 }//end while
Vigilance88 25:49ccdc98639a 267
Vigilance88 27:0cefe83f83d3 268 //TO DO:
Vigilance88 27:0cefe83f83d3 269 //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 / (2*pi/4200) )
Vigilance88 27:0cefe83f83d3 270 //Encoder1.setPulses(100); //edited QEI library: added setPulses()
Vigilance88 27:0cefe83f83d3 271 //Encoder2.setPulses(100); //edited QEI library: added setPulses()
Vigilance88 27:0cefe83f83d3 272 //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses());
Vigilance88 27:0cefe83f83d3 273 wait(1);
Vigilance88 27:0cefe83f83d3 274 pc.printf("Arm Calibration Complete\r\n");
Vigilance88 18:44905b008f44 275 }
Vigilance88 18:44905b008f44 276
Vigilance88 18:44905b008f44 277
Vigilance88 18:44905b008f44 278 //Input error and Kp, Kd, Ki, output control signal
Vigilance88 20:0ede3818e08e 279 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
vsluiter 2:e314bb3b2d99 280 {
Vigilance88 20:0ede3818e08e 281 // Derivative
Vigilance88 24:56db31267f10 282 double e_der = (error-e_prev)/ CONTROL_RATE;
Vigilance88 28:0460786f9573 283 e_der = derfilter.step(e_der);
Vigilance88 21:d6a46315d5f5 284 e_prev = error;
Vigilance88 20:0ede3818e08e 285 // Integral
Vigilance88 24:56db31267f10 286 e_int = e_int + CONTROL_RATE * error;
Vigilance88 20:0ede3818e08e 287 // PID
Vigilance88 32:054900bfb0a5 288 return kp*error + ki*e_int + kd * e_der;
Vigilance88 30:ec66691d226d 289
Vigilance88 30:ec66691d226d 290 }
Vigilance88 30:ec66691d226d 291
Vigilance88 30:ec66691d226d 292 double pid2(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
Vigilance88 30:ec66691d226d 293 {
Vigilance88 30:ec66691d226d 294 // Derivative
Vigilance88 30:ec66691d226d 295 double e_der = (error-e_prev)/ CONTROL_RATE;
Vigilance88 30:ec66691d226d 296 e_der = derfilter2.step(e_der);
Vigilance88 30:ec66691d226d 297 e_prev = error;
Vigilance88 30:ec66691d226d 298 // Integral
Vigilance88 30:ec66691d226d 299 e_int = e_int + CONTROL_RATE * error;
Vigilance88 30:ec66691d226d 300 // PID
Vigilance88 30:ec66691d226d 301 return kp*error + ki*e_int; //+ kd * e_der;
Vigilance88 27:0cefe83f83d3 302
Vigilance88 18:44905b008f44 303 }
Vigilance88 18:44905b008f44 304
Vigilance88 27:0cefe83f83d3 305 void controlMenu()
Vigilance88 27:0cefe83f83d3 306 {
Vigilance88 27:0cefe83f83d3 307 pc.printf("1) Move Arm Left\r\n");
Vigilance88 27:0cefe83f83d3 308 pc.printf("2) Move Arm Right\r\n");
Vigilance88 27:0cefe83f83d3 309 pc.printf("3) Move Arm Up\r\n");
Vigilance88 27:0cefe83f83d3 310 pc.printf("4) Move Arm Down\r\n");
Vigilance88 27:0cefe83f83d3 311 pc.printf("q) Exit \r\n");
Vigilance88 27:0cefe83f83d3 312 pc.printf("Please make a choice => \r\n");
Vigilance88 27:0cefe83f83d3 313 }
Vigilance88 27:0cefe83f83d3 314
Vigilance88 27:0cefe83f83d3 315
Vigilance88 27:0cefe83f83d3 316
Vigilance88 20:0ede3818e08e 317 //Analyze filtered EMG, calculate reference position from EMG, compare reference position with current position,convert to angles, send error through pid(), send PWM and DIR to motors
Vigilance88 18:44905b008f44 318 void control()
Vigilance88 18:44905b008f44 319 {
Vigilance88 27:0cefe83f83d3 320
Vigilance88 27:0cefe83f83d3 321 //Current position - Encoder counts -> current angle -> Forward Kinematics
Vigilance88 27:0cefe83f83d3 322 rpc=(2*PI)/ENCODER1_CPR; //radians per count (resolution) - 2pi divided by 4200
Vigilance88 27:0cefe83f83d3 323 theta1 = Encoder1.getPulses() * rpc; //multiply resolution with number of counts
Vigilance88 27:0cefe83f83d3 324 theta2 = Encoder2.getPulses() * rpc;
Vigilance88 27:0cefe83f83d3 325 current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
Vigilance88 27:0cefe83f83d3 326 current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
Vigilance88 27:0cefe83f83d3 327
Vigilance88 27:0cefe83f83d3 328 //pc.printf("Calculated current position: x = %f and y = %f \r\n",current_x,current_y);
Vigilance88 18:44905b008f44 329
Vigilance88 27:0cefe83f83d3 330
Vigilance88 27:0cefe83f83d3 331 //pc.printf("X is %f and Y is %f \r\n",x,y);
Vigilance88 27:0cefe83f83d3 332
Vigilance88 27:0cefe83f83d3 333 //calculate error (refpos-currentpos) currentpos = forward kinematics
Vigilance88 27:0cefe83f83d3 334 x_error = x - current_x;
Vigilance88 27:0cefe83f83d3 335 y_error = y - current_y;
Vigilance88 27:0cefe83f83d3 336
Vigilance88 27:0cefe83f83d3 337 //pc.printf("X error is %f and Y error is %f \r\n",x_error,y_error);
Vigilance88 27:0cefe83f83d3 338
Vigilance88 27:0cefe83f83d3 339 //inverse kinematics (refpos to refangle)
Vigilance88 18:44905b008f44 340
Vigilance88 27:0cefe83f83d3 341 costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
Vigilance88 27:0cefe83f83d3 342 sintheta2 = sqrt( 1 - pow(costheta2,2) );
Vigilance88 27:0cefe83f83d3 343
Vigilance88 27:0cefe83f83d3 344 //pc.printf("costheta2 = %f and sostheta2 = %f \r\n",costheta2,sostheta2);
Vigilance88 27:0cefe83f83d3 345
Vigilance88 27:0cefe83f83d3 346 dtheta2 = atan2(sintheta2,costheta2);
Vigilance88 27:0cefe83f83d3 347
Vigilance88 28:0460786f9573 348 costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sintheta2 ) / ( pow(x,2) + pow(y,2) );
Vigilance88 27:0cefe83f83d3 349 sintheta1 = sqrt( 1 - pow(costheta1,2) );
Vigilance88 18:44905b008f44 350
Vigilance88 27:0cefe83f83d3 351 //pc.printf("costheta1 = %f and sostheta1 = %f \r\n",costheta1,sostheta1);
Vigilance88 27:0cefe83f83d3 352
Vigilance88 27:0cefe83f83d3 353 dtheta1 = atan2(sintheta1,costheta1);
Vigilance88 27:0cefe83f83d3 354
Vigilance88 27:0cefe83f83d3 355
Vigilance88 27:0cefe83f83d3 356 //Angle error
Vigilance88 27:0cefe83f83d3 357
Vigilance88 27:0cefe83f83d3 358 m1_error = dtheta1-theta1;
Vigilance88 27:0cefe83f83d3 359 m2_error = dtheta2-theta2;
Vigilance88 27:0cefe83f83d3 360
Vigilance88 27:0cefe83f83d3 361 //pc.printf("m1 error is %f and m2 error is %f \r\n",m1_error,m2_error);
Vigilance88 18:44905b008f44 362
Vigilance88 18:44905b008f44 363 //PID controller
Vigilance88 24:56db31267f10 364
Vigilance88 24:56db31267f10 365 u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1
Vigilance88 24:56db31267f10 366 u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2
Vigilance88 21:d6a46315d5f5 367
Vigilance88 28:0460786f9573 368 keep_in_range(&u1,-0.6,0.6); //keep u between -1 and 1, sign = direction, PWM = 0-1
Vigilance88 28:0460786f9573 369 keep_in_range(&u2,-0.6,0.6);
Vigilance88 21:d6a46315d5f5 370
Vigilance88 21:d6a46315d5f5 371 //send PWM and DIR to motor 1
Vigilance88 28:0460786f9573 372 dir_motor1 = u1>0 ? 1 : 0; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below.
Vigilance88 21:d6a46315d5f5 373 pwm_motor1.write(abs(u1));
Vigilance88 21:d6a46315d5f5 374
Vigilance88 21:d6a46315d5f5 375 //send PWM and DIR to motor 2
Vigilance88 28:0460786f9573 376 dir_motor2 = u2>0 ? 0 : 1; //conditional statement, same as if else below
Vigilance88 25:49ccdc98639a 377 pwm_motor2.write(abs(u2));
Vigilance88 21:d6a46315d5f5 378
Vigilance88 27:0cefe83f83d3 379
Vigilance88 27:0cefe83f83d3 380 // } //if
Vigilance88 27:0cefe83f83d3 381 //} //while
Vigilance88 27:0cefe83f83d3 382
Vigilance88 27:0cefe83f83d3 383
Vigilance88 21:d6a46315d5f5 384 /*if(u1 > 0)
Vigilance88 21:d6a46315d5f5 385 {
Vigilance88 21:d6a46315d5f5 386 dir_motor1 = 0;
Vigilance88 21:d6a46315d5f5 387 else{
Vigilance88 21:d6a46315d5f5 388 dir_motor1 = 1;
Vigilance88 21:d6a46315d5f5 389 }
Vigilance88 21:d6a46315d5f5 390 }
Vigilance88 21:d6a46315d5f5 391 pwm_motor1.write(abs(u1));
Vigilance88 21:d6a46315d5f5 392
Vigilance88 21:d6a46315d5f5 393
Vigilance88 21:d6a46315d5f5 394 if(u2 > 0)
Vigilance88 21:d6a46315d5f5 395 {
Vigilance88 21:d6a46315d5f5 396 dir_motor1 = 1;
Vigilance88 21:d6a46315d5f5 397 else{
Vigilance88 21:d6a46315d5f5 398 dir_motor1 = 0;
Vigilance88 21:d6a46315d5f5 399 }
Vigilance88 21:d6a46315d5f5 400 }
Vigilance88 21:d6a46315d5f5 401 pwm_motor1.write(abs(u2));*/
Vigilance88 21:d6a46315d5f5 402
Vigilance88 18:44905b008f44 403 }
Vigilance88 18:44905b008f44 404
Vigilance88 26:fe3a5469dd6b 405 void mainMenu()
Vigilance88 26:fe3a5469dd6b 406 {
Vigilance88 26:fe3a5469dd6b 407 //Title Box
Vigilance88 26:fe3a5469dd6b 408 pc.putc(201);
Vigilance88 26:fe3a5469dd6b 409 for(int j=0;j<33;j++){
Vigilance88 26:fe3a5469dd6b 410 pc.putc(205);
Vigilance88 26:fe3a5469dd6b 411 }
Vigilance88 26:fe3a5469dd6b 412 pc.putc(187);
Vigilance88 26:fe3a5469dd6b 413 pc.printf("\n\r");
Vigilance88 26:fe3a5469dd6b 414 pc.putc(186); pc.printf(" BioRobotics M9 - Group 14 "); pc.putc(186);
Vigilance88 26:fe3a5469dd6b 415 pc.printf("\n\r");
Vigilance88 26:fe3a5469dd6b 416 pc.putc(186); pc.printf(" Robot powered ON "); pc.putc(186);
Vigilance88 26:fe3a5469dd6b 417 pc.printf("\n\r");
Vigilance88 26:fe3a5469dd6b 418 pc.putc(200);
Vigilance88 26:fe3a5469dd6b 419 for(int k=0;k<33;k++){
Vigilance88 26:fe3a5469dd6b 420 pc.putc(205);
Vigilance88 26:fe3a5469dd6b 421 }
Vigilance88 26:fe3a5469dd6b 422 pc.putc(188);
Vigilance88 26:fe3a5469dd6b 423
Vigilance88 26:fe3a5469dd6b 424 pc.printf("\n\r");
Vigilance88 26:fe3a5469dd6b 425 //endbox
Vigilance88 26:fe3a5469dd6b 426 }
Vigilance88 24:56db31267f10 427 void caliMenu(){};
Vigilance88 24:56db31267f10 428
Vigilance88 24:56db31267f10 429
Vigilance88 24:56db31267f10 430
Vigilance88 18:44905b008f44 431 //Start control
Vigilance88 18:44905b008f44 432 void start_control(void)
Vigilance88 18:44905b008f44 433 {
Vigilance88 27:0cefe83f83d3 434 control_timer.attach(&control,0.01); //100 Hz control
Vigilance88 26:fe3a5469dd6b 435
Vigilance88 26:fe3a5469dd6b 436 //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan.
Vigilance88 26:fe3a5469dd6b 437 blue=0;
Vigilance88 26:fe3a5469dd6b 438 pc.printf("||- started control -|| \r\n");
Vigilance88 18:44905b008f44 439 }
Vigilance88 18:44905b008f44 440
Vigilance88 18:44905b008f44 441 //stop control
Vigilance88 18:44905b008f44 442 void stop_control(void)
Vigilance88 18:44905b008f44 443 {
Vigilance88 18:44905b008f44 444 control_timer.detach();
Vigilance88 26:fe3a5469dd6b 445
Vigilance88 26:fe3a5469dd6b 446 //Debug LED will be off when control is off
Vigilance88 26:fe3a5469dd6b 447 blue=1;
Vigilance88 26:fe3a5469dd6b 448 pc.printf("||- stopped control -|| \r\n");
vsluiter 2:e314bb3b2d99 449 }
vsluiter 0:32bb76391d89 450
Vigilance88 21:d6a46315d5f5 451
Vigilance88 21:d6a46315d5f5 452 void calibrate()
vsluiter 0:32bb76391d89 453 {
Vigilance88 21:d6a46315d5f5 454
Vigilance88 21:d6a46315d5f5 455 }
tomlankhorst 15:0da764eea774 456
Vigilance88 26:fe3a5469dd6b 457 //Clears the putty (or other terminal) window
Vigilance88 26:fe3a5469dd6b 458 void clearTerminal()
Vigilance88 26:fe3a5469dd6b 459 {
Vigilance88 26:fe3a5469dd6b 460 pc.putc(27);
Vigilance88 26:fe3a5469dd6b 461 pc.printf("[2J"); // clear screen
Vigilance88 26:fe3a5469dd6b 462 pc.putc(27); // ESC
Vigilance88 26:fe3a5469dd6b 463 pc.printf("[H"); // cursor to home
Vigilance88 26:fe3a5469dd6b 464 }
Vigilance88 21:d6a46315d5f5 465
Vigilance88 21:d6a46315d5f5 466 //keeps input limited between min max
Vigilance88 24:56db31267f10 467 void keep_in_range(double * in, double min, double max)
Vigilance88 18:44905b008f44 468 {
Vigilance88 18:44905b008f44 469 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:32bb76391d89 470 }