Biorobotics Project / Mbed 2 deprecated Motor_poscontrol

not working yet

Dependencies:   Motor_with_encoder MODSERIAL mbed HIDScope

main.cpp@9:d4927ec5862f, 2017-10-23 (annotated)

Committer:
EvB
Date:
Mon Oct 23 10:06:37 2017 +0000
Revision:
9:d4927ec5862f
Parent:
8:9edf32e021a5
Child:
10:39a97906fa4b
Motor controllable by EMG signal.

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 8:9edf32e021a5 11 // ---- EMG parameters ----
EvB 9:d4927ec5862f 12 HIDScope scope (2);
vera1 8:9edf32e021a5 13 EMG_filter emg1(A0);
EvB 9:d4927ec5862f 14 float emgthreshold = 0.20;
vera1 8:9edf32e021a5 15 // ------------------------
vera1 8:9edf32e021a5 16
vera1 8:9edf32e021a5 17 // ---- Motor input and outputs ----
MMartens 0:9167ae5d9927 18 PwmOut speed1(D5);
MMartens 4:75f6e4845194 19 PwmOut speed2(D6);
MMartens 0:9167ae5d9927 20 DigitalOut dir1(D4);
MMartens 4:75f6e4845194 21 DigitalOut dir2(D7);
vera1 6:fc46581fe3e0 22 DigitalIn press(PTA4);
MMartens 0:9167ae5d9927 23 DigitalOut led1(D8);
MMartens 0:9167ae5d9927 24 DigitalOut led2(D11);
MMartens 4:75f6e4845194 25 AnalogIn pot(A2);
MMartens 4:75f6e4845194 26 AnalogIn pot2(A1);
MMartens 0:9167ae5d9927 27 Encoder motor1(PTD0,PTC4);
MMartens 4:75f6e4845194 28 Encoder motor2(D12,D13);
vera1 8:9edf32e021a5 29 // ----------------------------------
vera1 8:9edf32e021a5 30
vera1 8:9edf32e021a5 31 // --- Define Ticker and Timeout ---
vera1 8:9edf32e021a5 32 Ticker mainticker; //Ticker that calls every function. Functions are called by means of a variable named go, e.g. go_EMG
vera1 8:9edf32e021a5 33 Timeout calibrationgo; // Timeout that will determine the duration of the calibration.
vera1 8:9edf32e021a5 34 // ---------------------------------
vera1 8:9edf32e021a5 35
MMartens 0:9167ae5d9927 36
MMartens 1:f3fe6d2b7639 37 float PwmPeriod = 0.0001f;
MMartens 1:f3fe6d2b7639 38
MMartens 0:9167ae5d9927 39 double count = 0; //set the counts of the encoder
MMartens 1:f3fe6d2b7639 40 volatile double angle = 0;//set the angles
MMartens 0:9167ae5d9927 41
MMartens 4:75f6e4845194 42 double count2 = 0; //set the counts of the encoder
MMartens 4:75f6e4845194 43 volatile double angle2 = 0;//set the angles
MMartens 4:75f6e4845194 44
MMartens 3:e888f52a46bc 45 double setpoint = 6.28;//I am setting it to move through 180 degrees
MMartens 4:75f6e4845194 46 double setpoint2 = 6.28;//I am setting it to move through 180 degrees
MMartens 3:e888f52a46bc 47 double Kp = 250;// you can set these constants however you like depending on trial & error
MMartens 3:e888f52a46bc 48 double Ki = 100;
MMartens 2:7c6391c8ca71 49 double Kd = 0;
MMartens 0:9167ae5d9927 50
MMartens 0:9167ae5d9927 51 float last_error = 0;
MMartens 1:f3fe6d2b7639 52 float error1 = 0;
MMartens 0:9167ae5d9927 53 float changeError = 0;
MMartens 0:9167ae5d9927 54 float totalError = 0;
MMartens 0:9167ae5d9927 55 float pidTerm = 0;
MMartens 4:75f6e4845194 56 float pidTerm_scaled = 0;
MMartens 4:75f6e4845194 57
MMartens 4:75f6e4845194 58 float last_error2 = 0;
MMartens 4:75f6e4845194 59 float error2 = 0;
MMartens 4:75f6e4845194 60 float changeError2 = 0;
MMartens 4:75f6e4845194 61 float totalError2 = 0;
MMartens 4:75f6e4845194 62 float pidTerm2 = 0;
MMartens 4:75f6e4845194 63 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 64
MMartens 0:9167ae5d9927 65 volatile double potvalue = 0.0;
MMartens 4:75f6e4845194 66 volatile double potvalue2 = 0.0;
MMartens 0:9167ae5d9927 67 volatile double position = 0.0;
MMartens 4:75f6e4845194 68 volatile double position2 = 0.0;
MMartens 3:e888f52a46bc 69
vera1 8:9edf32e021a5 70 // --- Booleans for the maintickerfunction ---
vera1 6:fc46581fe3e0 71 //bool readoutsetpoint = true;
vera1 8:9edf32e021a5 72 bool go_EMG; // Boolean to put on/off the EMG readout
vera1 8:9edf32e021a5 73 bool go_calibration; // Boolean to put on/off the calibration of the EMG
vera1 8:9edf32e021a5 74 // -------------------------------------------
vera1 8:9edf32e021a5 75
vera1 8:9edf32e021a5 76 // --- calibrate EMG signal ----
vera1 8:9edf32e021a5 77
vera1 8:9edf32e021a5 78 void calibrationGO() // Function for go or no go of calibration
vera1 8:9edf32e021a5 79 {
vera1 8:9edf32e021a5 80 go_calibration = false;
vera1 8:9edf32e021a5 81 led2 = 0;
vera1 8:9edf32e021a5 82 }
vera1 8:9edf32e021a5 83
vera1 8:9edf32e021a5 84 /*
vera1 8:9edf32e021a5 85 Calibration of the robot works as follows:
vera1 8:9edf32e021a5 86 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 87 The value of the MVC is overwritten when the value of the filtered signal is bigger than the current value.
vera1 8:9edf32e021a5 88 During the calibration the user should exert maximum force.
vera1 8:9edf32e021a5 89 */
vera1 8:9edf32e021a5 90 void calibrationEMG() // Function for calibrating the EMG signal
vera1 8:9edf32e021a5 91 {
vera1 8:9edf32e021a5 92 if (go_calibration)
vera1 8:9edf32e021a5 93 {
vera1 8:9edf32e021a5 94 led2 = 1;
EvB 9:d4927ec5862f 95 emg1.calibration(); // Using the calibration function of the EMG_filter class
vera1 8:9edf32e021a5 96 }
vera1 8:9edf32e021a5 97 }
vera1 8:9edf32e021a5 98
vera1 8:9edf32e021a5 99 // ------------------------------
MMartens 3:e888f52a46bc 100
EvB 9:d4927ec5862f 101
vera1 8:9edf32e021a5 102
vera1 8:9edf32e021a5 103 //Function that reads out the raw EMG and filters the signal
vera1 8:9edf32e021a5 104 void processEMG ()
vera1 8:9edf32e021a5 105 {
vera1 8:9edf32e021a5 106 led1 = 1;
EvB 9:d4927ec5862f 107
vera1 8:9edf32e021a5 108 //go_EMG = false; //set the variable to false --> misschien nodig?
vera1 8:9edf32e021a5 109 emg1.filter(); //filter the incoming emg signal
vera1 8:9edf32e021a5 110 //emg2.filter();
vera1 8:9edf32e021a5 111 //emg3.filter();
vera1 8:9edf32e021a5 112
vera1 8:9edf32e021a5 113 scope.set(0, emg1.normalized); //views emg1.normalized on port 0 of HIDScope
vera1 8:9edf32e021a5 114 scope.set(1, emg1.emg0);
EvB 9:d4927ec5862f 115 scope.send();
EvB 9:d4927ec5862f 116
EvB 9:d4927ec5862f 117 }
EvB 9:d4927ec5862f 118
EvB 9:d4927ec5862f 119 void setpointreadout()
EvB 9:d4927ec5862f 120 {
EvB 9:d4927ec5862f 121
EvB 9:d4927ec5862f 122 potvalue = pot.read();
EvB 9:d4927ec5862f 123 if (emg1.normalized>=emgthreshold)
EvB 9:d4927ec5862f 124 {
EvB 9:d4927ec5862f 125 setpoint = emg1.normalized*6.50; //setpoint in translational direction (cm). EMG output (0.2-1.0) scaled to maximal translation distance (52 cm).
EvB 9:d4927ec5862f 126 }
EvB 9:d4927ec5862f 127
EvB 9:d4927ec5862f 128 potvalue2 = pot2.read();
EvB 9:d4927ec5862f 129 setpoint2 = potvalue2*6.28f;
vera1 8:9edf32e021a5 130
vera1 8:9edf32e021a5 131 }
vera1 8:9edf32e021a5 132
vera1 8:9edf32e021a5 133
MMartens 2:7c6391c8ca71 134
MMartens 4:75f6e4845194 135 void PIDcalculation() // inputs: potvalue, motor#, setpoint
MMartens 1:f3fe6d2b7639 136 {
MMartens 5:8c6d66a7c5da 137 setpointreadout();
EvB 9:d4927ec5862f 138 angle = motor1.getPosition()/4200.00;
MMartens 4:75f6e4845194 139 angle2 = motor2.getPosition()/4200.00*6.28;
MMartens 5:8c6d66a7c5da 140
MMartens 1:f3fe6d2b7639 141 //pc.printf("pos: %d, speed %f reference position = %.2f\r ",motor1.getPosition(), motor1.getSpeed(), setpoint);
MMartens 0:9167ae5d9927 142 //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 143
MMartens 3:e888f52a46bc 144 error1 = setpoint - angle;
MMartens 4:75f6e4845194 145 error2 = setpoint2 - angle2;
vera1 6:fc46581fe3e0 146
MMartens 3:e888f52a46bc 147 changeError = (error1 - last_error)/0.001f; // derivative term
MMartens 3:e888f52a46bc 148 totalError += error1*0.001f; //accumalate errors to find integral term
MMartens 3:e888f52a46bc 149 pidTerm = (Kp * error1) + (Ki * totalError) + (Kd * changeError);//total gain
MMartens 3:e888f52a46bc 150 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 151 if (pidTerm >= 1000) {
MMartens 3:e888f52a46bc 152 pidTerm = 1000;
MMartens 3:e888f52a46bc 153 } else if (pidTerm <= -1000) {
MMartens 3:e888f52a46bc 154 pidTerm = -1000;
MMartens 3:e888f52a46bc 155 } else {
MMartens 3:e888f52a46bc 156 pidTerm = pidTerm;
MMartens 3:e888f52a46bc 157 }
MMartens 3:e888f52a46bc 158 //constraining to appropriate value
MMartens 3:e888f52a46bc 159 if (pidTerm >= 0) {
MMartens 1:f3fe6d2b7639 160 dir1 = 1;// Forward motion
MMartens 1:f3fe6d2b7639 161 } else {
MMartens 1:f3fe6d2b7639 162 dir1 = 0;//Reverse motion
MMartens 1:f3fe6d2b7639 163 }
MMartens 3:e888f52a46bc 164 pidTerm_scaled = abs(pidTerm)/1000.0f; //make sure it's a positive value
MMartens 3:e888f52a46bc 165 if(pidTerm_scaled >= 0.55f){
MMartens 3:e888f52a46bc 166 pidTerm_scaled = 0.55f;
MMartens 1:f3fe6d2b7639 167 }
MMartens 4:75f6e4845194 168
MMartens 4:75f6e4845194 169 changeError2 = (error2 - last_error2)/0.001f; // derivative term
MMartens 4:75f6e4845194 170 totalError2 += error2*0.001f; //accumalate errors to find integral term
MMartens 4:75f6e4845194 171 pidTerm2 = (Kp * error2) + (Ki * totalError2) + (Kd * changeError2);//total gain
MMartens 4:75f6e4845194 172 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 173 if (pidTerm2 >= 1000) {
MMartens 4:75f6e4845194 174 pidTerm2 = 1000;
MMartens 4:75f6e4845194 175 } else if (pidTerm2 <= -1000) {
MMartens 4:75f6e4845194 176 pidTerm2 = -1000;
MMartens 4:75f6e4845194 177 } else {
MMartens 4:75f6e4845194 178 pidTerm2 = pidTerm2;
MMartens 4:75f6e4845194 179 }
MMartens 4:75f6e4845194 180 //constraining to appropriate value
MMartens 4:75f6e4845194 181 if (pidTerm2 >= 0) {
MMartens 4:75f6e4845194 182 dir2 = 1;// Forward motion
MMartens 4:75f6e4845194 183 } else {
MMartens 4:75f6e4845194 184 dir2 = 0;//Reverse motion
MMartens 4:75f6e4845194 185 }
MMartens 4:75f6e4845194 186 pidTerm_scaled2 = abs(pidTerm2)/1000.0f; //make sure it's a positive value
MMartens 4:75f6e4845194 187 if(pidTerm_scaled2 >= 0.55f){
MMartens 4:75f6e4845194 188 pidTerm_scaled2 = 0.55f;
MMartens 4:75f6e4845194 189 }
MMartens 3:e888f52a46bc 190
MMartens 1:f3fe6d2b7639 191 last_error = error1;
MMartens 3:e888f52a46bc 192 speed1 = pidTerm_scaled+0.45f;
MMartens 4:75f6e4845194 193 last_error2 = error2;
MMartens 4:75f6e4845194 194 speed2 = pidTerm_scaled2+0.45f;
MMartens 0:9167ae5d9927 195 }
MMartens 0:9167ae5d9927 196
vera1 8:9edf32e021a5 197 void maintickerfunction()
vera1 8:9edf32e021a5 198 {
vera1 8:9edf32e021a5 199 if(go_EMG)
vera1 8:9edf32e021a5 200 {
vera1 8:9edf32e021a5 201 processEMG();
vera1 8:9edf32e021a5 202 }
vera1 8:9edf32e021a5 203
vera1 8:9edf32e021a5 204 PIDcalculation();
vera1 8:9edf32e021a5 205 }
vera1 8:9edf32e021a5 206
MMartens 1:f3fe6d2b7639 207 int main()
vera1 8:9edf32e021a5 208 {
vera1 8:9edf32e021a5 209 go_EMG = true; // Setting ticker variables
vera1 8:9edf32e021a5 210 go_calibration = true; // Setting the timeout variable
MMartens 1:f3fe6d2b7639 211 speed1.period(PwmPeriod);
MMartens 4:75f6e4845194 212 speed2.period(PwmPeriod);
MMartens 3:e888f52a46bc 213
vera1 8:9edf32e021a5 214 calibrationgo.attach(&calibrationGO, 5.0); // Attach calibration timeout to calibration function
vera1 8:9edf32e021a5 215 mainticker.attach(&calibrationEMG, 0.001f); // Attach ticker to the calibration function
vera1 8:9edf32e021a5 216 wait(5.0f);
vera1 8:9edf32e021a5 217
vera1 8:9edf32e021a5 218 mainticker.attach(&maintickerfunction,0.001f);
vera1 8:9edf32e021a5 219
vera1 6:fc46581fe3e0 220 int count = 0;
MMartens 1:f3fe6d2b7639 221 while(true) {
MMartens 3:e888f52a46bc 222
MMartens 3:e888f52a46bc 223 count++;
MMartens 3:e888f52a46bc 224 if(count == 100){
MMartens 3:e888f52a46bc 225 count = 0;
MMartens 3:e888f52a46bc 226 pc.printf("Angle = %f, pidTerm = %f ,PID_scaled = %f, Error = %f, setpoint = %f\r\n", angle, pidTerm ,pidTerm_scaled, error1,setpoint);
EvB 9:d4927ec5862f 227 // 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 228 }
MMartens 5:8c6d66a7c5da 229
vera1 6:fc46581fe3e0 230
vera1 6:fc46581fe3e0 231 wait(0.001f);
vera1 6:fc46581fe3e0 232 }
MMartens 4:75f6e4845194 233
MMartens 4:75f6e4845194 234
MMartens 4:75f6e4845194 235
MMartens 4:75f6e4845194 236
vera1 6:fc46581fe3e0 237
vera1 6:fc46581fe3e0 238
MMartens 1:f3fe6d2b7639 239
MMartens 0:9167ae5d9927 240 }
MMartens 0:9167ae5d9927 241
MMartens 0:9167ae5d9927 242