Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@12:5be2001abe62, 2017-10-27 (annotated)

Committer:
vera1
Date:
Fri Oct 27 13:43:28 2017 +0000
Revision:
12:5be2001abe62
Parent:
11:bda77916d2ea
Child:
13:acd120520e80
trying to implement servocontrol. servo doesn't react yet

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