group14 - k9 / Mbed 2 deprecated testPID

testcode pid

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of testPID by Martijn Kern

main.cpp@27:0cefe83f83d3, 2015-10-18 (annotated)

Committer:
Vigilance88
Date:
Sun Oct 18 23:24:30 2015 +0000
Revision:
27:0cefe83f83d3
Parent:
26:fe3a5469dd6b
Child:
28:0460786f9573
update calibrate_arm()

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 25:49ccdc98639a 26 //Debug legs
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 27:0cefe83f83d3 68 const double m1_kp=0.001; const double m1_ki=1; const double m1_kd=0; //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 27:0cefe83f83d3 71 const double m2_kp=0.001; const double m2_ki=2; const double m2_kd=0; //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 24:56db31267f10 100
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 18:44905b008f44 120
Vigilance88 26:fe3a5469dd6b 121 //Menu functions
Vigilance88 21:d6a46315d5f5 122 void mainMenu();
Vigilance88 21:d6a46315d5f5 123 void caliMenu();
Vigilance88 26:fe3a5469dd6b 124 void clearTerminal();
Vigilance88 27:0cefe83f83d3 125 void controlMenu();
Vigilance88 21:d6a46315d5f5 126
Vigilance88 21:d6a46315d5f5 127 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 128 ---- MAIN LOOP -------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 129 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 21:d6a46315d5f5 130
Vigilance88 21:d6a46315d5f5 131 int main()
Vigilance88 21:d6a46315d5f5 132 {
Vigilance88 26:fe3a5469dd6b 133 pc.baud(115200); //terminal baudrate
Vigilance88 26:fe3a5469dd6b 134 red=1; green=1; blue=1; //Make sure debug LEDS are off
Vigilance88 26:fe3a5469dd6b 135
Vigilance88 27:0cefe83f83d3 136 //Set PwmOut frequency to 10k Hz???
Vigilance88 27:0cefe83f83d3 137 //pwm_motor1.period(0.01);
Vigilance88 27:0cefe83f83d3 138 //pwm_motor2.period(0.01);
Vigilance88 27:0cefe83f83d3 139
Vigilance88 27:0cefe83f83d3 140 dir_motor1.write(1); //for motor 1 - 1 = counterclockwise
Vigilance88 27:0cefe83f83d3 141 dir_motor2.write(1); //for motor 2 - 1 = clockwise
Vigilance88 27:0cefe83f83d3 142 pwm_motor1=0;
Vigilance88 27:0cefe83f83d3 143 pwm_motor2=0;
Vigilance88 27:0cefe83f83d3 144
Vigilance88 26:fe3a5469dd6b 145
Vigilance88 26:fe3a5469dd6b 146 clearTerminal(); //Clear the putty window
Vigilance88 26:fe3a5469dd6b 147
Vigilance88 24:56db31267f10 148 // make a menu, user has to initiate next step
Vigilance88 26:fe3a5469dd6b 149 mainMenu();
Vigilance88 25:49ccdc98639a 150 //caliMenu(); // Menu function
Vigilance88 25:49ccdc98639a 151 //calibrate_arm(); //Start Calibration
Vigilance88 25:49ccdc98639a 152
Vigilance88 27:0cefe83f83d3 153 //calibrate_emg();
Vigilance88 27:0cefe83f83d3 154 wait(1);
Vigilance88 27:0cefe83f83d3 155 start_control(); //100 Hz control
Vigilance88 21:d6a46315d5f5 156
Vigilance88 21:d6a46315d5f5 157 //maybe some stop commands when a button is pressed: detach from timers.
Vigilance88 21:d6a46315d5f5 158 //stop_control();
Vigilance88 21:d6a46315d5f5 159 //stop_sampling();
Vigilance88 27:0cefe83f83d3 160 char c;
Vigilance88 27:0cefe83f83d3 161 pc.printf("entering while loop \r\n");
Vigilance88 21:d6a46315d5f5 162 while(1) {
Vigilance88 27:0cefe83f83d3 163
Vigilance88 27:0cefe83f83d3 164 if( pc.readable() ){
Vigilance88 27:0cefe83f83d3 165 c = pc.getc();
Vigilance88 27:0cefe83f83d3 166 switch (c)
Vigilance88 27:0cefe83f83d3 167 {
Vigilance88 27:0cefe83f83d3 168 case '1' :
Vigilance88 27:0cefe83f83d3 169 x = x + 0.01;
Vigilance88 27:0cefe83f83d3 170 //controlMenu();
Vigilance88 27:0cefe83f83d3 171 //running=false;
Vigilance88 27:0cefe83f83d3 172 break;
Vigilance88 27:0cefe83f83d3 173
Vigilance88 27:0cefe83f83d3 174 case '2' :
Vigilance88 27:0cefe83f83d3 175 x-=0.01;
Vigilance88 27:0cefe83f83d3 176 //controlMenu();
Vigilance88 27:0cefe83f83d3 177 //running=false;
Vigilance88 27:0cefe83f83d3 178 break;
Vigilance88 27:0cefe83f83d3 179
Vigilance88 27:0cefe83f83d3 180 case '3' :
Vigilance88 27:0cefe83f83d3 181 y+=0.01;
Vigilance88 27:0cefe83f83d3 182 //controlMenu();
Vigilance88 27:0cefe83f83d3 183 //running=false;
Vigilance88 27:0cefe83f83d3 184 break;
Vigilance88 27:0cefe83f83d3 185
Vigilance88 27:0cefe83f83d3 186
Vigilance88 27:0cefe83f83d3 187 case '4' :
Vigilance88 27:0cefe83f83d3 188 y-=0.01;
Vigilance88 27:0cefe83f83d3 189 //controlMenu();
Vigilance88 27:0cefe83f83d3 190 //running=false;
Vigilance88 27:0cefe83f83d3 191 break;
Vigilance88 27:0cefe83f83d3 192
Vigilance88 27:0cefe83f83d3 193 case 'q' :
Vigilance88 27:0cefe83f83d3 194 pc.printf("Quit");
Vigilance88 27:0cefe83f83d3 195 //running = false;
Vigilance88 27:0cefe83f83d3 196 break;
Vigilance88 27:0cefe83f83d3 197 }//end switch
Vigilance88 27:0cefe83f83d3 198 pc.printf("Reference position: %f and %f \r\n",x,y);
Vigilance88 27:0cefe83f83d3 199 pc.printf("Current position: %f and %f \r\n",current_x,current_y);
Vigilance88 27:0cefe83f83d3 200 pc.printf("Current angles: %f and %f \r\n",theta1,theta2);
Vigilance88 27:0cefe83f83d3 201 pc.printf("Error in angles: %f and %f \r\n",dtheta1,dtheta2);
Vigilance88 27:0cefe83f83d3 202 pc.printf("PID output: %f and %f \r\n",u1,u2);
Vigilance88 27:0cefe83f83d3 203 pc.printf("----------------------------------------\r\n\n");
Vigilance88 27:0cefe83f83d3 204 }
Vigilance88 27:0cefe83f83d3 205 //end if
Vigilance88 21:d6a46315d5f5 206 }
Vigilance88 21:d6a46315d5f5 207 //end of while loop
Vigilance88 21:d6a46315d5f5 208 }
Vigilance88 21:d6a46315d5f5 209 //end of main
Vigilance88 21:d6a46315d5f5 210
Vigilance88 21:d6a46315d5f5 211 /*--------------------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 212 ---- FUNCTIONS -------------------------------------------------------------------------------------------------------
Vigilance88 21:d6a46315d5f5 213 --------------------------------------------------------------------------------------------------------------------*/
Vigilance88 18:44905b008f44 214
Vigilance88 27:0cefe83f83d3 215
Vigilance88 18:44905b008f44 216
Vigilance88 18:44905b008f44 217 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
Vigilance88 19:0a3ee31dcdb4 218 void calibrate_arm(void)
Vigilance88 19:0a3ee31dcdb4 219 {
Vigilance88 27:0cefe83f83d3 220 pc.printf("Calibrate_arm() entered\r\n");
Vigilance88 26:fe3a5469dd6b 221 bool calibrating = true;
Vigilance88 26:fe3a5469dd6b 222 bool done1 = false;
Vigilance88 26:fe3a5469dd6b 223 bool done2 = false;
Vigilance88 27:0cefe83f83d3 224 pc.printf("To start arm calibration, press any key =>");
Vigilance88 27:0cefe83f83d3 225 pc.getc();
Vigilance88 27:0cefe83f83d3 226 pc.printf("\r\n Calibrating... \r\n");
Vigilance88 26:fe3a5469dd6b 227 dir_motor1=1; //cw
Vigilance88 27:0cefe83f83d3 228 dir_motor2=0; //cw
Vigilance88 27:0cefe83f83d3 229 pwm_motor1.write(0.2); //move upper arm slowly cw
Vigilance88 27:0cefe83f83d3 230
Vigilance88 27:0cefe83f83d3 231
Vigilance88 26:fe3a5469dd6b 232
Vigilance88 26:fe3a5469dd6b 233 while(calibrating){
Vigilance88 27:0cefe83f83d3 234 red=0; blue=0; //Debug light is purple during arm calibration
Vigilance88 27:0cefe83f83d3 235
Vigilance88 27:0cefe83f83d3 236 if(done1==true){
Vigilance88 27:0cefe83f83d3 237 pwm_motor2.write(0.2); //move forearm slowly cw
Vigilance88 27:0cefe83f83d3 238 }
Vigilance88 27:0cefe83f83d3 239
Vigilance88 26:fe3a5469dd6b 240 //when limit switches are hit, stop motor and reset encoder
Vigilance88 26:fe3a5469dd6b 241 if(shoulder_limit.read() < 0.5){ //polling
Vigilance88 26:fe3a5469dd6b 242 pwm_motor1.write(0);
Vigilance88 26:fe3a5469dd6b 243 Encoder1.reset();
Vigilance88 26:fe3a5469dd6b 244 done1 = true;
Vigilance88 27:0cefe83f83d3 245 pc.printf("Shoulder Limit hit - shutting down motor 1\r\n");
Vigilance88 26:fe3a5469dd6b 246 }
Vigilance88 26:fe3a5469dd6b 247 if(elbow_limit.read() < 0.5){ //polling
Vigilance88 26:fe3a5469dd6b 248 pwm_motor2.write(0);
Vigilance88 26:fe3a5469dd6b 249 Encoder2.reset();
Vigilance88 26:fe3a5469dd6b 250 done2 = true;
Vigilance88 27:0cefe83f83d3 251 pc.printf("Elbow Limit hit - shutting down motor 2. \r\n");
Vigilance88 26:fe3a5469dd6b 252 }
Vigilance88 26:fe3a5469dd6b 253 if(done1 && done2){
Vigilance88 26:fe3a5469dd6b 254 calibrating=false; //Leave while loop when both limits are reached
Vigilance88 26:fe3a5469dd6b 255 red=1; blue=1; //Turn debug light off when calibration complete
Vigilance88 26:fe3a5469dd6b 256 }
Vigilance88 27:0cefe83f83d3 257
Vigilance88 26:fe3a5469dd6b 258 }//end while
Vigilance88 25:49ccdc98639a 259
Vigilance88 27:0cefe83f83d3 260 //TO DO:
Vigilance88 27:0cefe83f83d3 261 //mechanical angle limits -> pulses. If 40 degrees -> counts = floor( 40 / (2*pi/4200) )
Vigilance88 27:0cefe83f83d3 262 //Encoder1.setPulses(100); //edited QEI library: added setPulses()
Vigilance88 27:0cefe83f83d3 263 //Encoder2.setPulses(100); //edited QEI library: added setPulses()
Vigilance88 27:0cefe83f83d3 264 //pc.printf("Elbow Limit hit - shutting down motor 2. Current pulsecount: %i \r\n",Encoder1.getPulses());
Vigilance88 27:0cefe83f83d3 265 wait(1);
Vigilance88 27:0cefe83f83d3 266 pc.printf("Arm Calibration Complete\r\n");
Vigilance88 18:44905b008f44 267 }
Vigilance88 18:44905b008f44 268
Vigilance88 18:44905b008f44 269
Vigilance88 18:44905b008f44 270 //Input error and Kp, Kd, Ki, output control signal
Vigilance88 20:0ede3818e08e 271 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
vsluiter 2:e314bb3b2d99 272 {
Vigilance88 20:0ede3818e08e 273 // Derivative
Vigilance88 24:56db31267f10 274 double e_der = (error-e_prev)/ CONTROL_RATE;
Vigilance88 27:0cefe83f83d3 275 //e_der = derfilter.step(e_der);
Vigilance88 21:d6a46315d5f5 276 e_prev = error;
Vigilance88 20:0ede3818e08e 277 // Integral
Vigilance88 24:56db31267f10 278 e_int = e_int + CONTROL_RATE * error;
Vigilance88 20:0ede3818e08e 279 // PID
Vigilance88 27:0cefe83f83d3 280 return kp*error + ki*e_int;
Vigilance88 27:0cefe83f83d3 281
Vigilance88 27:0cefe83f83d3 282 //+ kd * e_der;
Vigilance88 18:44905b008f44 283 }
Vigilance88 18:44905b008f44 284
Vigilance88 27:0cefe83f83d3 285 void controlMenu()
Vigilance88 27:0cefe83f83d3 286 {
Vigilance88 27:0cefe83f83d3 287 pc.printf("1) Move Arm Left\r\n");
Vigilance88 27:0cefe83f83d3 288 pc.printf("2) Move Arm Right\r\n");
Vigilance88 27:0cefe83f83d3 289 pc.printf("3) Move Arm Up\r\n");
Vigilance88 27:0cefe83f83d3 290 pc.printf("4) Move Arm Down\r\n");
Vigilance88 27:0cefe83f83d3 291 pc.printf("q) Exit \r\n");
Vigilance88 27:0cefe83f83d3 292 pc.printf("Please make a choice => \r\n");
Vigilance88 27:0cefe83f83d3 293 }
Vigilance88 27:0cefe83f83d3 294
Vigilance88 27:0cefe83f83d3 295
Vigilance88 27:0cefe83f83d3 296
Vigilance88 20:0ede3818e08e 297 //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 298 void control()
Vigilance88 18:44905b008f44 299 {
Vigilance88 27:0cefe83f83d3 300
Vigilance88 27:0cefe83f83d3 301 //Current position - Encoder counts -> current angle -> Forward Kinematics
Vigilance88 27:0cefe83f83d3 302 rpc=(2*PI)/ENCODER1_CPR; //radians per count (resolution) - 2pi divided by 4200
Vigilance88 27:0cefe83f83d3 303 theta1 = Encoder1.getPulses() * rpc; //multiply resolution with number of counts
Vigilance88 27:0cefe83f83d3 304 theta2 = Encoder2.getPulses() * rpc;
Vigilance88 27:0cefe83f83d3 305 current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2);
Vigilance88 27:0cefe83f83d3 306 current_y = l1 * sin(theta1) + l2 * sin(theta1 + theta2);
Vigilance88 27:0cefe83f83d3 307
Vigilance88 27:0cefe83f83d3 308 //pc.printf("Calculated current position: x = %f and y = %f \r\n",current_x,current_y);
Vigilance88 18:44905b008f44 309
Vigilance88 27:0cefe83f83d3 310
Vigilance88 27:0cefe83f83d3 311 //pc.printf("X is %f and Y is %f \r\n",x,y);
Vigilance88 27:0cefe83f83d3 312
Vigilance88 27:0cefe83f83d3 313 //calculate error (refpos-currentpos) currentpos = forward kinematics
Vigilance88 27:0cefe83f83d3 314 x_error = x - current_x;
Vigilance88 27:0cefe83f83d3 315 y_error = y - current_y;
Vigilance88 27:0cefe83f83d3 316
Vigilance88 27:0cefe83f83d3 317 //pc.printf("X error is %f and Y error is %f \r\n",x_error,y_error);
Vigilance88 27:0cefe83f83d3 318
Vigilance88 27:0cefe83f83d3 319 //inverse kinematics (refpos to refangle)
Vigilance88 18:44905b008f44 320
Vigilance88 27:0cefe83f83d3 321 costheta2 = (pow(x,2) + pow(y,2) - pow(l1,2) - pow(l2,2)) / (2*l1*l2) ;
Vigilance88 27:0cefe83f83d3 322 sintheta2 = sqrt( 1 - pow(costheta2,2) );
Vigilance88 27:0cefe83f83d3 323
Vigilance88 27:0cefe83f83d3 324 //pc.printf("costheta2 = %f and sostheta2 = %f \r\n",costheta2,sostheta2);
Vigilance88 27:0cefe83f83d3 325
Vigilance88 27:0cefe83f83d3 326 dtheta2 = atan2(sintheta2,costheta2);
Vigilance88 27:0cefe83f83d3 327
Vigilance88 27:0cefe83f83d3 328 costheta1 = ( x * (l1 + l2 * costheta2) + y * l2 * sostheta2 ) / ( pow(x,2) + pow(y,2) );
Vigilance88 27:0cefe83f83d3 329 sintheta1 = sqrt( 1 - pow(costheta1,2) );
Vigilance88 18:44905b008f44 330
Vigilance88 27:0cefe83f83d3 331 //pc.printf("costheta1 = %f and sostheta1 = %f \r\n",costheta1,sostheta1);
Vigilance88 27:0cefe83f83d3 332
Vigilance88 27:0cefe83f83d3 333 dtheta1 = atan2(sintheta1,costheta1);
Vigilance88 27:0cefe83f83d3 334
Vigilance88 27:0cefe83f83d3 335
Vigilance88 27:0cefe83f83d3 336 //Angle error
Vigilance88 27:0cefe83f83d3 337
Vigilance88 27:0cefe83f83d3 338 m1_error = dtheta1-theta1;
Vigilance88 27:0cefe83f83d3 339 m2_error = dtheta2-theta2;
Vigilance88 27:0cefe83f83d3 340
Vigilance88 27:0cefe83f83d3 341 //pc.printf("m1 error is %f and m2 error is %f \r\n",m1_error,m2_error);
Vigilance88 18:44905b008f44 342
Vigilance88 18:44905b008f44 343 //PID controller
Vigilance88 24:56db31267f10 344
Vigilance88 24:56db31267f10 345 u1=pid(m1_error,m1_kp,m1_ki,m1_kd,m1_e_int,m1_e_prev); //motor 1
Vigilance88 24:56db31267f10 346 u2=pid(m2_error,m2_kp,m2_ki,m2_kd,m2_e_int,m2_e_prev); //motor 2
Vigilance88 21:d6a46315d5f5 347
Vigilance88 21:d6a46315d5f5 348 keep_in_range(&u1,-1,1); //keep u between -1 and 1, sign = direction, PWM = 0-1
Vigilance88 21:d6a46315d5f5 349 keep_in_range(&u2,-1,1);
Vigilance88 21:d6a46315d5f5 350
Vigilance88 21:d6a46315d5f5 351 //send PWM and DIR to motor 1
Vigilance88 27:0cefe83f83d3 352 dir_motor1 = u1>0 ? 0 : 1; //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below.
Vigilance88 21:d6a46315d5f5 353 pwm_motor1.write(abs(u1));
Vigilance88 21:d6a46315d5f5 354
Vigilance88 21:d6a46315d5f5 355 //send PWM and DIR to motor 2
Vigilance88 25:49ccdc98639a 356 dir_motor2 = u2>0 ? 1 : 0; //conditional statement, same as if else below
Vigilance88 25:49ccdc98639a 357 pwm_motor2.write(abs(u2));
Vigilance88 21:d6a46315d5f5 358
Vigilance88 27:0cefe83f83d3 359
Vigilance88 27:0cefe83f83d3 360 // } //if
Vigilance88 27:0cefe83f83d3 361 //} //while
Vigilance88 27:0cefe83f83d3 362
Vigilance88 27:0cefe83f83d3 363
Vigilance88 21:d6a46315d5f5 364 /*if(u1 > 0)
Vigilance88 21:d6a46315d5f5 365 {
Vigilance88 21:d6a46315d5f5 366 dir_motor1 = 0;
Vigilance88 21:d6a46315d5f5 367 else{
Vigilance88 21:d6a46315d5f5 368 dir_motor1 = 1;
Vigilance88 21:d6a46315d5f5 369 }
Vigilance88 21:d6a46315d5f5 370 }
Vigilance88 21:d6a46315d5f5 371 pwm_motor1.write(abs(u1));
Vigilance88 21:d6a46315d5f5 372
Vigilance88 21:d6a46315d5f5 373
Vigilance88 21:d6a46315d5f5 374 if(u2 > 0)
Vigilance88 21:d6a46315d5f5 375 {
Vigilance88 21:d6a46315d5f5 376 dir_motor1 = 1;
Vigilance88 21:d6a46315d5f5 377 else{
Vigilance88 21:d6a46315d5f5 378 dir_motor1 = 0;
Vigilance88 21:d6a46315d5f5 379 }
Vigilance88 21:d6a46315d5f5 380 }
Vigilance88 21:d6a46315d5f5 381 pwm_motor1.write(abs(u2));*/
Vigilance88 21:d6a46315d5f5 382
Vigilance88 18:44905b008f44 383 }
Vigilance88 18:44905b008f44 384
Vigilance88 26:fe3a5469dd6b 385 void mainMenu()
Vigilance88 26:fe3a5469dd6b 386 {
Vigilance88 26:fe3a5469dd6b 387 //Title Box
Vigilance88 26:fe3a5469dd6b 388 pc.putc(201);
Vigilance88 26:fe3a5469dd6b 389 for(int j=0;j<33;j++){
Vigilance88 26:fe3a5469dd6b 390 pc.putc(205);
Vigilance88 26:fe3a5469dd6b 391 }
Vigilance88 26:fe3a5469dd6b 392 pc.putc(187);
Vigilance88 26:fe3a5469dd6b 393 pc.printf("\n\r");
Vigilance88 26:fe3a5469dd6b 394 pc.putc(186); pc.printf(" BioRobotics M9 - Group 14 "); pc.putc(186);
Vigilance88 26:fe3a5469dd6b 395 pc.printf("\n\r");
Vigilance88 26:fe3a5469dd6b 396 pc.putc(186); pc.printf(" Robot powered ON "); pc.putc(186);
Vigilance88 26:fe3a5469dd6b 397 pc.printf("\n\r");
Vigilance88 26:fe3a5469dd6b 398 pc.putc(200);
Vigilance88 26:fe3a5469dd6b 399 for(int k=0;k<33;k++){
Vigilance88 26:fe3a5469dd6b 400 pc.putc(205);
Vigilance88 26:fe3a5469dd6b 401 }
Vigilance88 26:fe3a5469dd6b 402 pc.putc(188);
Vigilance88 26:fe3a5469dd6b 403
Vigilance88 26:fe3a5469dd6b 404 pc.printf("\n\r");
Vigilance88 26:fe3a5469dd6b 405 //endbox
Vigilance88 26:fe3a5469dd6b 406 }
Vigilance88 24:56db31267f10 407 void caliMenu(){};
Vigilance88 24:56db31267f10 408
Vigilance88 24:56db31267f10 409
Vigilance88 24:56db31267f10 410
Vigilance88 18:44905b008f44 411 //Start control
Vigilance88 18:44905b008f44 412 void start_control(void)
Vigilance88 18:44905b008f44 413 {
Vigilance88 27:0cefe83f83d3 414 control_timer.attach(&control,0.01); //100 Hz control
Vigilance88 26:fe3a5469dd6b 415
Vigilance88 26:fe3a5469dd6b 416 //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan.
Vigilance88 26:fe3a5469dd6b 417 blue=0;
Vigilance88 26:fe3a5469dd6b 418 pc.printf("||- started control -|| \r\n");
Vigilance88 18:44905b008f44 419 }
Vigilance88 18:44905b008f44 420
Vigilance88 18:44905b008f44 421 //stop control
Vigilance88 18:44905b008f44 422 void stop_control(void)
Vigilance88 18:44905b008f44 423 {
Vigilance88 18:44905b008f44 424 control_timer.detach();
Vigilance88 26:fe3a5469dd6b 425
Vigilance88 26:fe3a5469dd6b 426 //Debug LED will be off when control is off
Vigilance88 26:fe3a5469dd6b 427 blue=1;
Vigilance88 26:fe3a5469dd6b 428 pc.printf("||- stopped control -|| \r\n");
vsluiter 2:e314bb3b2d99 429 }
vsluiter 0:32bb76391d89 430
Vigilance88 21:d6a46315d5f5 431
Vigilance88 21:d6a46315d5f5 432 void calibrate()
vsluiter 0:32bb76391d89 433 {
Vigilance88 21:d6a46315d5f5 434
Vigilance88 21:d6a46315d5f5 435 }
tomlankhorst 15:0da764eea774 436
Vigilance88 26:fe3a5469dd6b 437 //Clears the putty (or other terminal) window
Vigilance88 26:fe3a5469dd6b 438 void clearTerminal()
Vigilance88 26:fe3a5469dd6b 439 {
Vigilance88 26:fe3a5469dd6b 440 pc.putc(27);
Vigilance88 26:fe3a5469dd6b 441 pc.printf("[2J"); // clear screen
Vigilance88 26:fe3a5469dd6b 442 pc.putc(27); // ESC
Vigilance88 26:fe3a5469dd6b 443 pc.printf("[H"); // cursor to home
Vigilance88 26:fe3a5469dd6b 444 }
Vigilance88 21:d6a46315d5f5 445
Vigilance88 21:d6a46315d5f5 446 //keeps input limited between min max
Vigilance88 24:56db31267f10 447 void keep_in_range(double * in, double min, double max)
Vigilance88 18:44905b008f44 448 {
Vigilance88 18:44905b008f44 449 *in > min ? *in < max? : *in = max: *in = min;
vsluiter 0:32bb76391d89 450 }