Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@11:bda77916d2ea, 2017-10-27 (annotated)

Committer:
vera1
Date:
Fri Oct 27 10:00:20 2017 +0000
Revision:
11:bda77916d2ea
Parent:
8:9edf32e021a5
Child:
12:5be2001abe62
Switch implemented, works but motors rotate as well during double arm activation;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MMartens 0:9167ae5d9927 1 #include "mbed.h"
MMartens 0:9167ae5d9927 2 #include "MODSERIAL.h"
MMartens 0:9167ae5d9927 3 #include "encoder.h"
MMartens 0:9167ae5d9927 4 #include "math.h"
MMartens 3:e888f52a46bc 5 #include "HIDScope.h"
vera1 8:9edf32e021a5 6 #include "BiQuad.h"
vera1 8:9edf32e021a5 7 #include "EMG_filter.h"
MMartens 0:9167ae5d9927 8
MMartens 0:9167ae5d9927 9 MODSERIAL pc(USBTX,USBRX);
vera1 8:9edf32e021a5 10
vera1 11:bda77916d2ea 11
vera1 11:bda77916d2ea 12 enum r_states {R_HORIZONTAL, R_VERTICAL, R_BASE};
vera1 11:bda77916d2ea 13 r_states state;
vera1 11:bda77916d2ea 14
vera1 8:9edf32e021a5 15 // ---- EMG parameters ----
vera1 8:9edf32e021a5 16 //HIDScope scope (2);
vera1 8:9edf32e021a5 17 EMG_filter emg1(A0);
vera1 11:bda77916d2ea 18 EMG_filter emg2(A1);
vera1 8:9edf32e021a5 19 // ------------------------
vera1 8:9edf32e021a5 20
vera1 8:9edf32e021a5 21 // ---- Motor input and outputs ----
MMartens 0:9167ae5d9927 22 PwmOut speed1(D5);
MMartens 4:75f6e4845194 23 PwmOut speed2(D6);
MMartens 0:9167ae5d9927 24 DigitalOut dir1(D4);
MMartens 4:75f6e4845194 25 DigitalOut dir2(D7);
vera1 6:fc46581fe3e0 26 DigitalIn press(PTA4);
vera1 11:bda77916d2ea 27 DigitalOut ledred(LED_RED);
vera1 11:bda77916d2ea 28 DigitalOut ledgreen(LED_GREEN);
vera1 11:bda77916d2ea 29 DigitalOut ledblue(LED_BLUE);
vera1 11:bda77916d2ea 30 DigitalOut ledstateswitch(D8);
vera1 11:bda77916d2ea 31 DigitalOut ledstatedef(D11);
MMartens 4:75f6e4845194 32 AnalogIn pot(A2);
vera1 11:bda77916d2ea 33 AnalogIn pot2(A3);
MMartens 0:9167ae5d9927 34 Encoder motor1(PTD0,PTC4);
MMartens 4:75f6e4845194 35 Encoder motor2(D12,D13);
vera1 8:9edf32e021a5 36 // ----------------------------------
vera1 8:9edf32e021a5 37
vera1 8:9edf32e021a5 38 // --- Define Ticker and Timeout ---
vera1 8:9edf32e021a5 39 Ticker mainticker; //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG
vera1 8:9edf32e021a5 40 Timeout calibrationgo; // Timeout that will determine the duration of the calibration.
vera1 8:9edf32e021a5 41 // ---------------------------------
vera1 8:9edf32e021a5 42
MMartens 0:9167ae5d9927 43
MMartens 1:f3fe6d2b7639 44 float PwmPeriod = 0.0001f;
MMartens 1:f3fe6d2b7639 45
MMartens 0:9167ae5d9927 46 double count = 0; //set the counts of the encoder
MMartens 1:f3fe6d2b7639 47 volatile double angle = 0;//set the angles
MMartens 0:9167ae5d9927 48
MMartens 4:75f6e4845194 49 double count2 = 0; //set the counts of the encoder
MMartens 4:75f6e4845194 50 volatile double angle2 = 0;//set the angles
MMartens 4:75f6e4845194 51
MMartens 3:e888f52a46bc 52 double setpoint = 6.28;//I am setting it to move through 180 degrees
MMartens 4:75f6e4845194 53 double setpoint2 = 6.28;//I am setting it to move through 180 degrees
vera1 11:bda77916d2ea 54 double Kp = 500;// you can set these constants however you like depending on trial & error
MMartens 3:e888f52a46bc 55 double Ki = 100;
MMartens 2:7c6391c8ca71 56 double Kd = 0;
MMartens 0:9167ae5d9927 57
MMartens 0:9167ae5d9927 58 float last_error = 0;
MMartens 1:f3fe6d2b7639 59 float error1 = 0;
MMartens 0:9167ae5d9927 60 float changeError = 0;
MMartens 0:9167ae5d9927 61 float totalError = 0;
MMartens 0:9167ae5d9927 62 float pidTerm = 0;
MMartens 4:75f6e4845194 63 float pidTerm_scaled = 0;
MMartens 4:75f6e4845194 64
MMartens 4:75f6e4845194 65 float last_error2 = 0;
MMartens 4:75f6e4845194 66 float error2 = 0;
MMartens 4:75f6e4845194 67 float changeError2 = 0;
MMartens 4:75f6e4845194 68 float totalError2 = 0;
MMartens 4:75f6e4845194 69 float pidTerm2 = 0;
MMartens 4:75f6e4845194 70 float pidTerm_scaled2 = 0;// if the total gain we get is not in the PWM range we scale it down so that it's not bigger than |255|
MMartens 0:9167ae5d9927 71
MMartens 0:9167ae5d9927 72 volatile double potvalue = 0.0;
MMartens 4:75f6e4845194 73 volatile double potvalue2 = 0.0;
MMartens 0:9167ae5d9927 74 volatile double position = 0.0;
MMartens 4:75f6e4845194 75 volatile double position2 = 0.0;
MMartens 3:e888f52a46bc 76
vera1 8:9edf32e021a5 77 // --- Booleans for the maintickerfunction ---
vera1 6:fc46581fe3e0 78 //bool readoutsetpoint = true;
vera1 8:9edf32e021a5 79 bool go_EMG; // Boolean to put on/off the EMG readout
vera1 11:bda77916d2ea 80 bool go_calibration; // Boolean to put on/off the calibration of the EMG
vera1 11:bda77916d2ea 81 bool go_switch; // Boolean to go to different state
vera1 11:bda77916d2ea 82 bool go_PID; // Boolean to calculate PID and motor aansturing
vera1 8:9edf32e021a5 83 // -------------------------------------------
vera1 8:9edf32e021a5 84
vera1 8:9edf32e021a5 85 // --- calibrate EMG signal ----
vera1 8:9edf32e021a5 86
vera1 8:9edf32e021a5 87 void calibrationGO() // Function for go or no go of calibration
vera1 8:9edf32e021a5 88 {
vera1 8:9edf32e021a5 89 go_calibration = false;
vera1 11:bda77916d2ea 90
vera1 8:9edf32e021a5 91 }
vera1 8:9edf32e021a5 92
vera1 8:9edf32e021a5 93 /*
vera1 8:9edf32e021a5 94 Calibration of the robot works as follows:
vera1 8:9edf32e021a5 95 EMG signal is read out for 5 seconds, 1000 times a second. This signal is filtered and the maximum value is compared to the current value of the MVC.
vera1 8:9edf32e021a5 96 The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
vera1 8:9edf32e021a5 97 During the calibration the user should exert maximum force.
vera1 8:9edf32e021a5 98 */
vera1 8:9edf32e021a5 99 void calibrationEMG() // Function for calibrating the EMG signal
vera1 8:9edf32e021a5 100 {
vera1 8:9edf32e021a5 101 if (go_calibration)
vera1 8:9edf32e021a5 102 {
vera1 11:bda77916d2ea 103
vera1 11:bda77916d2ea 104 emg1.calibration(); // Using the calibration function of the EMG_filter class
vera1 11:bda77916d2ea 105 emg2.calibration();
vera1 8:9edf32e021a5 106 }
vera1 8:9edf32e021a5 107 }
vera1 8:9edf32e021a5 108
vera1 8:9edf32e021a5 109 // ------------------------------
MMartens 3:e888f52a46bc 110
vera1 11:bda77916d2ea 111 // --- motor movements ---
vera1 11:bda77916d2ea 112 void r_movehorizontal()
vera1 11:bda77916d2ea 113 {
vera1 11:bda77916d2ea 114
vera1 11:bda77916d2ea 115
vera1 11:bda77916d2ea 116 }
vera1 11:bda77916d2ea 117
vera1 11:bda77916d2ea 118 void r_movevertical()
vera1 11:bda77916d2ea 119 {
vera1 11:bda77916d2ea 120
vera1 11:bda77916d2ea 121 }
vera1 11:bda77916d2ea 122
vera1 11:bda77916d2ea 123 void r_movebase()
vera1 11:bda77916d2ea 124 {
vera1 11:bda77916d2ea 125
vera1 11:bda77916d2ea 126 }
vera1 11:bda77916d2ea 127 //--------------------------------
vera1 11:bda77916d2ea 128
vera1 11:bda77916d2ea 129 //--- State switch function -----
vera1 11:bda77916d2ea 130 void r_processStateSwitch()
MMartens 5:8c6d66a7c5da 131 {
vera1 11:bda77916d2ea 132 if(go_switch) { //if go_switch is true state is switched
vera1 11:bda77916d2ea 133 go_switch = false;
vera1 11:bda77916d2ea 134 switch(state) {
vera1 11:bda77916d2ea 135 case R_HORIZONTAL:
vera1 11:bda77916d2ea 136 state = R_VERTICAL;
vera1 11:bda77916d2ea 137 ledblue = 1;
vera1 11:bda77916d2ea 138 ledred = 1;
vera1 11:bda77916d2ea 139 ledgreen = 0;
vera1 11:bda77916d2ea 140 pc.printf("state is vertical");
vera1 11:bda77916d2ea 141 break;
vera1 11:bda77916d2ea 142 case R_VERTICAL:
vera1 11:bda77916d2ea 143 state = R_BASE;
vera1 11:bda77916d2ea 144 ledgreen = 1;
vera1 11:bda77916d2ea 145 ledblue = 1;
vera1 11:bda77916d2ea 146 ledred = 0;
vera1 11:bda77916d2ea 147 pc.printf("state is base");
vera1 11:bda77916d2ea 148 break;
vera1 11:bda77916d2ea 149 case R_BASE:
vera1 11:bda77916d2ea 150 state = R_HORIZONTAL;
vera1 11:bda77916d2ea 151 ledgreen = 1;
vera1 11:bda77916d2ea 152 ledred = 1;
vera1 11:bda77916d2ea 153 ledblue = 0;
vera1 11:bda77916d2ea 154 pc.printf("state is horizontal");
vera1 11:bda77916d2ea 155 break;
vera1 11:bda77916d2ea 156 }
vera1 11:bda77916d2ea 157 wait(1.0f); // waits 1 second to continue with the main function. I think ticker does run, but main is on hold.
vera1 11:bda77916d2ea 158 ledstatedef = 1;
vera1 11:bda77916d2ea 159 ledstateswitch = 0;
vera1 11:bda77916d2ea 160 }
MMartens 5:8c6d66a7c5da 161 }
vera1 8:9edf32e021a5 162
vera1 8:9edf32e021a5 163 //Function that reads out the raw EMG and filters the signal
vera1 8:9edf32e021a5 164 void processEMG ()
vera1 8:9edf32e021a5 165 {
vera1 11:bda77916d2ea 166
vera1 11:bda77916d2ea 167
vera1 8:9edf32e021a5 168 emg1.filter(); //filter the incoming emg signal
vera1 11:bda77916d2ea 169 emg2.filter();
vera1 8:9edf32e021a5 170 //emg3.filter();
vera1 8:9edf32e021a5 171
vera1 11:bda77916d2ea 172 /* scope.set(0, emg1.normalized); //views emg1.normalized on port 0 of HIDScope
vera1 8:9edf32e021a5 173 scope.set(1, emg1.emg0);
vera1 8:9edf32e021a5 174 scope.send();*/
vera1 8:9edf32e021a5 175
vera1 8:9edf32e021a5 176 }
vera1 8:9edf32e021a5 177
vera1 11:bda77916d2ea 178 void setpointreadout()
vera1 11:bda77916d2ea 179 {
vera1 11:bda77916d2ea 180 potvalue = pot.read();
vera1 11:bda77916d2ea 181 setpoint = emg1.normalized;
vera1 11:bda77916d2ea 182
vera1 11:bda77916d2ea 183 potvalue2 = pot2.read();
vera1 11:bda77916d2ea 184 setpoint2 = emg2.normalized;
vera1 11:bda77916d2ea 185
vera1 11:bda77916d2ea 186 }
vera1 8:9edf32e021a5 187
MMartens 2:7c6391c8ca71 188
MMartens 4:75f6e4845194 189 void PIDcalculation() // inputs: potvalue, motor#, setpoint
MMartens 1:f3fe6d2b7639 190 {
MMartens 5:8c6d66a7c5da 191 setpointreadout();
vera1 11:bda77916d2ea 192 angle = motor1.getPosition()/4200.00;
MMartens 4:75f6e4845194 193 angle2 = motor2.getPosition()/4200.00*6.28;
MMartens 5:8c6d66a7c5da 194
MMartens 1:f3fe6d2b7639 195 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 196 //motorpid = PID(potvalue - position, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
MMartens 2:7c6391c8ca71 197
MMartens 3:e888f52a46bc 198 error1 = setpoint - angle;
MMartens 4:75f6e4845194 199 error2 = setpoint2 - angle2;
vera1 6:fc46581fe3e0 200
MMartens 3:e888f52a46bc 201 changeError = (error1 - last_error)/0.001f; // derivative term
MMartens 3:e888f52a46bc 202 totalError += error1*0.001f; //accumalate errors to find integral term
MMartens 3:e888f52a46bc 203 pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
MMartens 3:e888f52a46bc 204 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 205 if (pidTerm >= 1000) {
MMartens 3:e888f52a46bc 206 pidTerm = 1000;
MMartens 3:e888f52a46bc 207 } else if (pidTerm <= -1000) {
MMartens 3:e888f52a46bc 208 pidTerm = -1000;
MMartens 3:e888f52a46bc 209 } else {
MMartens 3:e888f52a46bc 210 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 211 }
MMartens 3:e888f52a46bc 212 //constraining to appropriate value
MMartens 3:e888f52a46bc 213 if (pidTerm >= 0) {
MMartens 1:f3fe6d2b7639 214 dir1 = 1;// Forward motion
MMartens 1:f3fe6d2b7639 215 } else {
MMartens 1:f3fe6d2b7639 216 dir1 = 0;//Reverse motion
MMartens 1:f3fe6d2b7639 217 }
MMartens 3:e888f52a46bc 218 pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
MMartens 3:e888f52a46bc 219 if(pidTerm_scaled >= 0.55f){
MMartens 3:e888f52a46bc 220 pidTerm_scaled = 0.55f;
MMartens 1:f3fe6d2b7639 221 }
MMartens 4:75f6e4845194 222
MMartens 4:75f6e4845194 223 changeError2 = (error2 - last_error2)/0.001f; // derivative term
MMartens 4:75f6e4845194 224 totalError2 += error2*0.001f; //accumalate errors to find integral term
MMartens 4:75f6e4845194 225 pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
MMartens 4:75f6e4845194 226 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 227 if (pidTerm2 >= 1000) {
MMartens 4:75f6e4845194 228 pidTerm2 = 1000;
MMartens 4:75f6e4845194 229 } else if (pidTerm2 <= -1000) {
MMartens 4:75f6e4845194 230 pidTerm2 = -1000;
MMartens 4:75f6e4845194 231 } else {
MMartens 4:75f6e4845194 232 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 233 }
MMartens 4:75f6e4845194 234 //constraining to appropriate value
MMartens 4:75f6e4845194 235 if (pidTerm2 >= 0) {
MMartens 4:75f6e4845194 236 dir2 = 1;// Forward motion
MMartens 4:75f6e4845194 237 } else {
MMartens 4:75f6e4845194 238 dir2 = 0;//Reverse motion
MMartens 4:75f6e4845194 239 }
MMartens 4:75f6e4845194 240 pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value
MMartens 4:75f6e4845194 241 if(pidTerm_scaled2 >= 0.55f){
MMartens 4:75f6e4845194 242 pidTerm_scaled2 = 0.55f;
MMartens 4:75f6e4845194 243 }
MMartens 3:e888f52a46bc 244
MMartens 1:f3fe6d2b7639 245 last_error = error1;
MMartens 3:e888f52a46bc 246 speed1 = pidTerm_scaled+0.45f;
MMartens 4:75f6e4845194 247 last_error2 = error2;
MMartens 4:75f6e4845194 248 speed2 = pidTerm_scaled2+0.45f;
MMartens 0:9167ae5d9927 249 }
MMartens 0:9167ae5d9927 250
vera1 8:9edf32e021a5 251 void maintickerfunction()
vera1 11:bda77916d2ea 252 {
vera1 11:bda77916d2ea 253 r_processStateSwitch();
vera1 11:bda77916d2ea 254
vera1 8:9edf32e021a5 255 if(go_EMG)
vera1 8:9edf32e021a5 256 {
vera1 8:9edf32e021a5 257 processEMG();
vera1 8:9edf32e021a5 258 }
vera1 11:bda77916d2ea 259 /*if(emg1.normalized >=0.2 && emg2.normalized >= 0.2) // PIDcalculation should not happen.
vera1 11:bda77916d2ea 260 {
vera1 11:bda77916d2ea 261 go_PID = false;
vera1 11:bda77916d2ea 262 }
vera1 11:bda77916d2ea 263 else{go_PID = true;}*/
vera1 8:9edf32e021a5 264
vera1 11:bda77916d2ea 265 if(go_PID){
vera1 11:bda77916d2ea 266 PIDcalculation();
vera1 11:bda77916d2ea 267 }
vera1 8:9edf32e021a5 268 }
vera1 8:9edf32e021a5 269
MMartens 1:f3fe6d2b7639 270 int main()
vera1 8:9edf32e021a5 271 {
vera1 11:bda77916d2ea 272 pc.baud(9600);
vera1 8:9edf32e021a5 273 go_EMG = true; // Setting ticker variables
vera1 8:9edf32e021a5 274 go_calibration = true; // Setting the timeout variable
vera1 11:bda77916d2ea 275 go_PID = true;
vera1 11:bda77916d2ea 276 go_switch = false;
MMartens 1:f3fe6d2b7639 277 speed1.period(PwmPeriod);
MMartens 4:75f6e4845194 278 speed2.period(PwmPeriod);
MMartens 3:e888f52a46bc 279
vera1 8:9edf32e021a5 280 calibrationgo.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
vera1 8:9edf32e021a5 281 mainticker.attach(&calibrationEMG, 0.001f); // Attach ticker to the calibration function
vera1 11:bda77916d2ea 282 wait(5.0f);
vera1 8:9edf32e021a5 283 mainticker.attach(&maintickerfunction,0.001f);
vera1 8:9edf32e021a5 284
vera1 11:bda77916d2ea 285 int count = 0;
vera1 11:bda77916d2ea 286
MMartens 1:f3fe6d2b7639 287 while(true) {
vera1 11:bda77916d2ea 288
vera1 11:bda77916d2ea 289 ledstatedef = 1;
vera1 11:bda77916d2ea 290 if(emg1.normalized >= 0.7 && emg2.normalized >= 0.7) {
vera1 11:bda77916d2ea 291 ledstateswitch = 1;
vera1 11:bda77916d2ea 292 ledstatedef = 0;
vera1 11:bda77916d2ea 293 go_switch = true;
vera1 11:bda77916d2ea 294
vera1 11:bda77916d2ea 295 }
MMartens 3:e888f52a46bc 296
vera1 11:bda77916d2ea 297
vera1 11:bda77916d2ea 298 switch(state) {
vera1 11:bda77916d2ea 299 case R_HORIZONTAL:
vera1 11:bda77916d2ea 300 r_movehorizontal();
vera1 11:bda77916d2ea 301 break;
vera1 11:bda77916d2ea 302 case R_VERTICAL:
vera1 11:bda77916d2ea 303 r_movevertical();
vera1 11:bda77916d2ea 304 break;
vera1 11:bda77916d2ea 305 case R_BASE:
vera1 11:bda77916d2ea 306 r_movebase();
vera1 11:bda77916d2ea 307 break;
vera1 11:bda77916d2ea 308 }
vera1 11:bda77916d2ea 309
MMartens 3:e888f52a46bc 310 count++;
MMartens 3:e888f52a46bc 311 if(count == 100){
MMartens 3:e888f52a46bc 312 count = 0;
vera1 11:bda77916d2ea 313 pc.printf("emg1 = %f, emg2 = %f\r\n", emg1.normalized, emg2.normalized);
vera1 11:bda77916d2ea 314 //pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1, setpoint);
vera1 11:bda77916d2ea 315 //pc.printf("Angle 2 = %f, pidTerm 2 = %f ,PID_scaled 2 = %f, Error 2 = %f, setpoint 2 = %f\r\n", angle2, pidTerm2 ,pidTerm_scaled2, error2,setpoint2);
MMartens 3:e888f52a46bc 316 }
MMartens 5:8c6d66a7c5da 317
vera1 6:fc46581fe3e0 318
vera1 6:fc46581fe3e0 319 wait(0.001f);
vera1 6:fc46581fe3e0 320 }
MMartens 4:75f6e4845194 321
MMartens 4:75f6e4845194 322
MMartens 4:75f6e4845194 323
MMartens 4:75f6e4845194 324
vera1 6:fc46581fe3e0 325
vera1 6:fc46581fe3e0 326
MMartens 1:f3fe6d2b7639 327
MMartens 0:9167ae5d9927 328 }
MMartens 0:9167ae5d9927 329
MMartens 0:9167ae5d9927 330