Roy van Zijl / Mbed 2 deprecated TestSubjectA

Mbed bordje 1 -af

Dependencies:   Encoder HIDScope MODSERIAL Matrix MatrixMath biquad-master mbed

Fork of dsjklafjaslkjdfalkjfdaslkasdjklajadsflkjdasflkjdasflkadsflkasd by Dion de Greef

main.cpp@5:3562c205d001, 2017-11-03 (annotated)

Committer:
RoyvZ
Date:
Fri Nov 03 06:40:08 2017 +0000
Revision:
5:3562c205d001
Parent:
4:afa85283eb18
Child:
6:643c44fb044a
MBed bordje 1 -> af;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RoyvZ 5:3562c205d001 1 //initialisation
RoyvZ 0:b6c8d56842ce 2 #include "mbed.h"
RoyvZ 5:3562c205d001 3 #include "encoder.h"
RoyvZ 0:b6c8d56842ce 4 #include "HIDScope.h"
RoyvZ 5:3562c205d001 5 #include "MODSERIAL.h"
RoyvZ 5:3562c205d001 6 #include "math.h"
RoyvZ 5:3562c205d001 7 #include "BiQuad.h"
RoyvZ 5:3562c205d001 8 #include "Matrix.h"
RoyvZ 5:3562c205d001 9 #include "MatrixMath.h"
RoyvZ 0:b6c8d56842ce 10 #include <stdlib.h>
RoyvZ 0:b6c8d56842ce 11 #include <iostream>
RoyvZ 0:b6c8d56842ce 12 #include <iomanip>
RoyvZ 0:b6c8d56842ce 13 #include <complex>
RoyvZ 3:2ffbee47fb38 14 #include <vector>
RoyvZ 5:3562c205d001 15
RoyvZ 5:3562c205d001 16 Ticker MainTicker;
RoyvZ 2:293665548183 17
RoyvZ 5:3562c205d001 18 //Define objects EMG
RoyvZ 5:3562c205d001 19 AnalogIn emg0( A0 );
RoyvZ 5:3562c205d001 20 AnalogIn emg1( A1 );
RoyvZ 5:3562c205d001 21 DigitalIn buttonCal(D11);
RoyvZ 5:3562c205d001 22 DigitalIn changeX(D2);
RoyvZ 5:3562c205d001 23 DigitalIn changeY(D3);
RoyvZ 5:3562c205d001 24 InterruptIn turning(D10);
RoyvZ 0:b6c8d56842ce 25
RoyvZ 5:3562c205d001 26 Ticker sample_timer;
RoyvZ 5:3562c205d001 27 Ticker calX_timer;
RoyvZ 5:3562c205d001 28 Ticker calY_timer;
RoyvZ 5:3562c205d001 29 //HIDScope scope( 4 );
RoyvZ 5:3562c205d001 30 DigitalOut led(LED1);
RoyvZ 5:3562c205d001 31 DigitalOut led2(LED_GREEN);
RoyvZ 5:3562c205d001 32
RoyvZ 5:3562c205d001 33 //Define variables EMG
RoyvZ 3:2ffbee47fb38 34 const int size = 100;
RoyvZ 5:3562c205d001 35 vector<double> SX(size,0);
RoyvZ 5:3562c205d001 36 vector<double> SY(size,0);
RoyvZ 3:2ffbee47fb38 37 double meanSum = 0;
RoyvZ 3:2ffbee47fb38 38 double maxsignal = 0;
RoyvZ 2:293665548183 39
RoyvZ 5:3562c205d001 40 //Filters
RoyvZ 2:293665548183 41 BiQuadChain Notch;
RoyvZ 3:2ffbee47fb38 42 BiQuadChain Notch50;
RoyvZ 3:2ffbee47fb38 43 BiQuadChain bqcLP;
RoyvZ 3:2ffbee47fb38 44 BiQuadChain bqc;
RoyvZ 0:b6c8d56842ce 45
RoyvZ 2:293665548183 46 //Notch filter chain for 100 Hz
RoyvZ 2:293665548183 47 BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 );
RoyvZ 2:293665548183 48 BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 );
RoyvZ 2:293665548183 49 BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 );
RoyvZ 2:293665548183 50
RoyvZ 2:293665548183 51 //Notch filter chain for 50 Hz
RoyvZ 3:2ffbee47fb38 52 BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 );
RoyvZ 3:2ffbee47fb38 53 BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 );
RoyvZ 3:2ffbee47fb38 54 BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 );
RoyvZ 2:293665548183 55
RoyvZ 5:3562c205d001 56 //2 Hz High Pass filter
RoyvZ 3:2ffbee47fb38 57 BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01);
RoyvZ 0:b6c8d56842ce 58
RoyvZ 5:3562c205d001 59 //80 Hz Low Pass filter
RoyvZ 3:2ffbee47fb38 60 BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 );
RoyvZ 3:2ffbee47fb38 61 BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 );
RoyvZ 3:2ffbee47fb38 62
RoyvZ 3:2ffbee47fb38 63
RoyvZ 5:3562c205d001 64 //Define objects MotorControl
RoyvZ 5:3562c205d001 65 DigitalOut gpo(D0);
RoyvZ 5:3562c205d001 66 DigitalOut UpperMotorDir(D4);
RoyvZ 5:3562c205d001 67 PwmOut UpperMotorVel(D5);
RoyvZ 5:3562c205d001 68 DigitalOut LowerMotorDir(D6);
RoyvZ 5:3562c205d001 69 PwmOut LowerMotorVel(D7);
RoyvZ 5:3562c205d001 70 AnalogIn potMeterIn(A1);
RoyvZ 5:3562c205d001 71 InterruptIn ButMotorDir(D3);
RoyvZ 5:3562c205d001 72 Encoder encoder1(D13,D12);
RoyvZ 5:3562c205d001 73 Encoder encoder2(D9,D8);
RoyvZ 5:3562c205d001 74 Ticker m1_Ticker;
RoyvZ 5:3562c205d001 75 Ticker setpoint_Ticker;
RoyvZ 0:b6c8d56842ce 76
RoyvZ 5:3562c205d001 77 //Define variables MotorControl
RoyvZ 5:3562c205d001 78 float M_PI = 3.14159265359;
RoyvZ 5:3562c205d001 79
RoyvZ 5:3562c205d001 80 float M1_KP = 1.0f;
RoyvZ 5:3562c205d001 81 float M1_KI = 0.1f;
RoyvZ 5:3562c205d001 82 const float M1_TS = 0.002;
RoyvZ 5:3562c205d001 83 const float q_RAD_PER_PULSE_m2 = 0.36f*M_PI/1300.0f; //2*M_PI/4320 voor als het nu geen rechte lijn meer is8
RoyvZ 5:3562c205d001 84 const float q_RAD_PER_PULSE_m1 = 0.5f*M_PI/3300.0f;
RoyvZ 5:3562c205d001 85
RoyvZ 5:3562c205d001 86 float m1_err_int = 0;
RoyvZ 5:3562c205d001 87 float m2_err_int = 0;
RoyvZ 5:3562c205d001 88 int motorD1 = 0;
RoyvZ 5:3562c205d001 89 float motor1 = 0;
RoyvZ 5:3562c205d001 90 int motorD2 = 0;
RoyvZ 5:3562c205d001 91 float motor2 = 0;
RoyvZ 5:3562c205d001 92 float position1 = 0;
RoyvZ 5:3562c205d001 93 float position2 = 0;
RoyvZ 5:3562c205d001 94
RoyvZ 5:3562c205d001 95 float emgX = 0;
RoyvZ 5:3562c205d001 96 float emgY = 0;
DiondeGreef 4:afa85283eb18 97
RoyvZ 5:3562c205d001 98 float L0 = 0.123;
RoyvZ 5:3562c205d001 99 float L1 = 0.37;
RoyvZ 5:3562c205d001 100 float L2 = 0.41;
RoyvZ 5:3562c205d001 101 float q1 = 0.0f*M_PI;
RoyvZ 5:3562c205d001 102 float q2 = 0.0f*M_PI;
RoyvZ 5:3562c205d001 103 float Periode = 0.001; //1000 is het aantal Hz
RoyvZ 5:3562c205d001 104
RoyvZ 5:3562c205d001 105 float ref_pos1;
RoyvZ 5:3562c205d001 106 float ref_pos2;
RoyvZ 5:3562c205d001 107
RoyvZ 5:3562c205d001 108 float Qset1 = 0;
RoyvZ 5:3562c205d001 109 float Qset2 = 0;
RoyvZ 5:3562c205d001 110
RoyvZ 5:3562c205d001 111 int UpperMotorPos;
RoyvZ 5:3562c205d001 112 int UpperMotorPos_prev;
RoyvZ 5:3562c205d001 113 int LowerMotorPos;
RoyvZ 5:3562c205d001 114 int LowerMotorPos_prev;
RoyvZ 5:3562c205d001 115
RoyvZ 5:3562c205d001 116 int NPC = 0;
RoyvZ 5:3562c205d001 117 float NoTurnies = 1;
RoyvZ 5:3562c205d001 118
RoyvZ 5:3562c205d001 119 //Define matrices MotorControl
RoyvZ 5:3562c205d001 120 Matrix JAPPAPP(2,2);
RoyvZ 5:3562c205d001 121 Matrix qdot(2,1);
RoyvZ 5:3562c205d001 122 Matrix Vdes(2,1);
RoyvZ 5:3562c205d001 123
RoyvZ 5:3562c205d001 124 //Define object Servo Motors
RoyvZ 5:3562c205d001 125 DigitalOut Ledr(LED_RED);
RoyvZ 5:3562c205d001 126 DigitalOut Ledg(LED_GREEN);
RoyvZ 5:3562c205d001 127 DigitalOut Ledb(LED_BLUE);
RoyvZ 5:3562c205d001 128 PwmOut LeftServoVel(D11);
RoyvZ 5:3562c205d001 129 PwmOut RightServoVel(D10);
RoyvZ 5:3562c205d001 130 DigitalIn ServoButton(D2);
RoyvZ 5:3562c205d001 131 Ticker ServoTick;
DiondeGreef 4:afa85283eb18 132
DiondeGreef 4:afa85283eb18 133
RoyvZ 5:3562c205d001 134 //Other variables
RoyvZ 5:3562c205d001 135 float Cal_Strength = 0.6f;
RoyvZ 5:3562c205d001 136
RoyvZ 5:3562c205d001 137
RoyvZ 5:3562c205d001 138 //Connection with TeraTerm
RoyvZ 5:3562c205d001 139 MODSERIAL pc(USBTX, USBRX);
RoyvZ 5:3562c205d001 140
RoyvZ 5:3562c205d001 141 //Creating of the states
RoyvZ 5:3562c205d001 142 enum states{calib1, calib2, calibEMG, UpDown, LeftRight, Vertical, MotionEndEffector, GrabbingPlacing, Off, Idle, No_State};
RoyvZ 5:3562c205d001 143
RoyvZ 5:3562c205d001 144 //Root Mean Square value calculator for useful envelope, using vectors
RoyvZ 5:3562c205d001 145 double findRMS(vector<double> array) {
RoyvZ 3:2ffbee47fb38 146 int i;
RoyvZ 5:3562c205d001 147 double sumsquared = 0.000; //define variable that resets to 0 for every call of the function
RoyvZ 3:2ffbee47fb38 148 double RMS;
RoyvZ 5:3562c205d001 149
RoyvZ 5:3562c205d001 150 for (i = 0; i < size; i++) //for loop to calculate the total sum of the squared values
RoyvZ 3:2ffbee47fb38 151 {
RoyvZ 3:2ffbee47fb38 152 sumsquared = sumsquared + array.at(i)*array.at(i);
RoyvZ 3:2ffbee47fb38 153 }
RoyvZ 5:3562c205d001 154 RMS = sqrt((double(1)/size)*(sumsquared)); //root mean square calculation
RoyvZ 3:2ffbee47fb38 155 return RMS;
RoyvZ 3:2ffbee47fb38 156 }
RoyvZ 5:3562c205d001 157
RoyvZ 5:3562c205d001 158
RoyvZ 5:3562c205d001 159 //Function to show the EMG signals in HIDScope
RoyvZ 5:3562c205d001 160 void sample() {
RoyvZ 0:b6c8d56842ce 161 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
RoyvZ 5:3562c205d001 162 //Standard emg value without filters, offset removed with the - value
RoyvZ 5:3562c205d001 163 //scope.set(0, emg0.read()-0.4542f);
RoyvZ 5:3562c205d001 164
RoyvZ 5:3562c205d001 165 //Use S vector to save values coming from EMG signal
RoyvZ 5:3562c205d001 166 SX.erase(SX.begin()); //remove first value
RoyvZ 5:3562c205d001 167 double emgXvalue = bqc.step(emg0.read()-0.4542f);
RoyvZ 5:3562c205d001 168 SX.push_back(emgXvalue); //add extra value to begin of vector
RoyvZ 5:3562c205d001 169
RoyvZ 5:3562c205d001 170 SY.erase(SY.begin()); //remove first value
RoyvZ 5:3562c205d001 171 double emgYvalue = bqc.step(emg0.read()-0.4542f);
RoyvZ 5:3562c205d001 172 SY.push_back(emgYvalue); //add extra value to begin of vector
RoyvZ 3:2ffbee47fb38 173
RoyvZ 3:2ffbee47fb38 174
RoyvZ 5:3562c205d001 175 //Calculate Root Mean Square value using findRMS function as defined above
RoyvZ 5:3562c205d001 176 double emgX = findRMS(SX);
RoyvZ 5:3562c205d001 177 double emgY = findRMS(SY);
RoyvZ 5:3562c205d001 178
RoyvZ 5:3562c205d001 179 //Display results
RoyvZ 5:3562c205d001 180 //scope.set(1, emgX);
RoyvZ 5:3562c205d001 181 //scope.set(2, emgY);
RoyvZ 5:3562c205d001 182
RoyvZ 5:3562c205d001 183 //Send all scopes to HIDScope
RoyvZ 5:3562c205d001 184 //scope.send();
RoyvZ 5:3562c205d001 185
RoyvZ 0:b6c8d56842ce 186 /* To indicate that the function is working, the LED is toggled */
RoyvZ 0:b6c8d56842ce 187 led = !led;
RoyvZ 0:b6c8d56842ce 188 }
RoyvZ 0:b6c8d56842ce 189
RoyvZ 5:3562c205d001 190 //Calibration function for the EMG signals
RoyvZ 5:3562c205d001 191 void calibrationX() {
RoyvZ 2:293665548183 192 //function to determine maximal EMG input in order to allow motorcontrol to be activated
RoyvZ 3:2ffbee47fb38 193
RoyvZ 5:3562c205d001 194 if (buttonCal.read() == false){ //activated when button is pressed
RoyvZ 3:2ffbee47fb38 195 led2 = !led2;
RoyvZ 2:293665548183 196 for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
RoyvZ 5:3562c205d001 197 SX.erase(SX.begin());
RoyvZ 5:3562c205d001 198 double emgXCalvalue = bqc.step(emg0.read()-0.4542f);
RoyvZ 5:3562c205d001 199 SX.push_back(emgXCalvalue);
RoyvZ 5:3562c205d001 200 double signalfinal = findRMS(SX); //calculate RMS values from EMG signals
RoyvZ 5:3562c205d001 201 if (signalfinal >= maxsignal){ //Check if current signal is higher than current maximal signal
RoyvZ 5:3562c205d001 202 maxsignal = signalfinal; //keep changing the maximal signal
RoyvZ 2:293665548183 203 }
RoyvZ 2:293665548183 204 }
RoyvZ 5:3562c205d001 205 led2 = 1; //Turn LED off when calibration is finished
RoyvZ 5:3562c205d001 206 }
RoyvZ 5:3562c205d001 207 }
RoyvZ 5:3562c205d001 208
RoyvZ 5:3562c205d001 209 void calibrationY() {
RoyvZ 5:3562c205d001 210 //function to determine maximal EMG input in order to allow motorcontrol to be activated
RoyvZ 5:3562c205d001 211
RoyvZ 5:3562c205d001 212 if (buttonCal.read() == false){ //activated when button is pressed
RoyvZ 5:3562c205d001 213 led2 = !led2;
RoyvZ 5:3562c205d001 214 for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
RoyvZ 5:3562c205d001 215 SY.erase(SY.begin());
RoyvZ 5:3562c205d001 216 double emgYCalvalue = bqc.step(emg1.read()-0.4542f);
RoyvZ 5:3562c205d001 217 SY.push_back(emgYCalvalue);
RoyvZ 5:3562c205d001 218 double signalfinal = findRMS(SY); //calculate RMS values from EMG signals
RoyvZ 5:3562c205d001 219 if (signalfinal >= maxsignal){ //Check if current signal is higher than current maximal signal
RoyvZ 5:3562c205d001 220 maxsignal = signalfinal; //keep changing the maximal signal
RoyvZ 5:3562c205d001 221 }
RoyvZ 5:3562c205d001 222 }
RoyvZ 5:3562c205d001 223 led2 = 1; //Turn LED off when calibration is finished
RoyvZ 5:3562c205d001 224 }
RoyvZ 5:3562c205d001 225 }
RoyvZ 5:3562c205d001 226
RoyvZ 5:3562c205d001 227
RoyvZ 5:3562c205d001 228 void NegPosChanger(){
RoyvZ 5:3562c205d001 229 if(NPC == 0){
RoyvZ 5:3562c205d001 230 NoTurnies = 0;
RoyvZ 5:3562c205d001 231 NPC++;
RoyvZ 5:3562c205d001 232 } else {
RoyvZ 5:3562c205d001 233 NoTurnies = 1;
RoyvZ 5:3562c205d001 234 NPC= 0;
RoyvZ 2:293665548183 235 }
RoyvZ 2:293665548183 236 }
RoyvZ 2:293665548183 237
DiondeGreef 4:afa85283eb18 238
RoyvZ 5:3562c205d001 239 void qSetpointSet() {
DiondeGreef 4:afa85283eb18 240 // Fill Matrix with data.
RoyvZ 5:3562c205d001 241 q1 = encoder1.getPosition()*q_RAD_PER_PULSE_m1; //Calibreren nog toevoegen
RoyvZ 5:3562c205d001 242 q2 = 0.5f*M_PI - q1 + encoder2.getPosition()*q_RAD_PER_PULSE_m2; //Calibreren mist nog
RoyvZ 5:3562c205d001 243
RoyvZ 5:3562c205d001 244 JAPPAPP(1,1) = (cos(q1)*cos(q2) - sin(q1)*sin(q2))/(L1*sin(q2)*cos(q1)*cos(q1) + L1*sin(q2)*sin(q1)*sin(q1));
RoyvZ 5:3562c205d001 245 JAPPAPP(1,2) = (cos(q1)*sin(q2) + cos(q2)*sin(q1))/(L1*sin(q2)*cos(q1)*cos(q1) + L1*sin(q2)*sin(q1)*sin(q1));
RoyvZ 5:3562c205d001 246 JAPPAPP(2,1) = -(L1*cos(q1) + L2*cos(q1)*cos(q2) - L2*sin(q1)*sin(q2))/(L1*L2*sin(q2)*cos(q1)*cos(q1) + L1*L2*sin(q2)*sin(q1)*sin(q1));
RoyvZ 5:3562c205d001 247 JAPPAPP(2,2) = -(L1*sin(q1) + L2*cos(q1)*sin(q2) + L2*cos(q2)*sin(q1))/(L1*L2*sin(q2)*cos(q1)*cos(q1) + L1*L2*sin(q2)*sin(q1)*sin(q1));
RoyvZ 5:3562c205d001 248
RoyvZ 5:3562c205d001 249 // Fill Matrix with data.
RoyvZ 5:3562c205d001 250 int Xchanger = (changeX == 1) ? -1 : 1;
RoyvZ 5:3562c205d001 251 int Ychanger = (changeY == 1) ? -1 : 1;
RoyvZ 5:3562c205d001 252 Vdes(1,1) = emgX*Xchanger*NoTurnies;
RoyvZ 5:3562c205d001 253 Vdes(2,1) = emgY*Ychanger*NoTurnies;
RoyvZ 5:3562c205d001 254 int turnChanger = (turning == 1) ? -1 : 1;
DiondeGreef 4:afa85283eb18 255
DiondeGreef 4:afa85283eb18 256 qdot = JAPPAPP*Vdes;
RoyvZ 5:3562c205d001 257
RoyvZ 5:3562c205d001 258 Qset1 = Qset1 + qdot(1,1)*Periode; //Vdes(1,1)
RoyvZ 5:3562c205d001 259 Qset2 = Qset2 + qdot(2,1)*Periode; //Vdes(2,1)
RoyvZ 5:3562c205d001 260
RoyvZ 5:3562c205d001 261 if (Qset1 <= 0.43f*M_PI && Qset1 >= 0.01f){
RoyvZ 5:3562c205d001 262 Qset1 = Qset1;
RoyvZ 5:3562c205d001 263 }
RoyvZ 5:3562c205d001 264 else if (Qset1 < 0.01f){
RoyvZ 5:3562c205d001 265 Qset1 = 0.01f;
RoyvZ 5:3562c205d001 266 }
RoyvZ 5:3562c205d001 267 else if (Qset1 > 0.43f*M_PI){
RoyvZ 5:3562c205d001 268 Qset1 = 0.43f*M_PI;
RoyvZ 5:3562c205d001 269 }
RoyvZ 5:3562c205d001 270 if (Qset1 + Qset2 < 0.5f*M_PI){
RoyvZ 5:3562c205d001 271 Qset2 = 0.5f*M_PI - Qset1;
RoyvZ 5:3562c205d001 272 }
RoyvZ 5:3562c205d001 273 if (Qset2 <= 0.5f*M_PI && Qset2 >= 0.1f){
RoyvZ 5:3562c205d001 274 Qset2 = Qset2;
RoyvZ 5:3562c205d001 275 }
RoyvZ 5:3562c205d001 276 else if (Qset2 < .1f){
RoyvZ 5:3562c205d001 277 Qset2 = 0.1f;
RoyvZ 5:3562c205d001 278 }
RoyvZ 5:3562c205d001 279 else if (Qset1+Qset2 > M_PI){
RoyvZ 5:3562c205d001 280 Qset2 = M_PI - Qset1;
RoyvZ 5:3562c205d001 281 }
RoyvZ 5:3562c205d001 282
RoyvZ 5:3562c205d001 283 }
RoyvZ 5:3562c205d001 284
RoyvZ 5:3562c205d001 285 float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ){ // Reusable PI controller
RoyvZ 5:3562c205d001 286 return fabs(Kp * e);
RoyvZ 5:3562c205d001 287 }
RoyvZ 5:3562c205d001 288
RoyvZ 5:3562c205d001 289 void PositionControl() {
RoyvZ 5:3562c205d001 290 //position1 = RAD_PER_PULSE * encoder1.getPosition();
RoyvZ 5:3562c205d001 291 //position2 = RAD_PER_PULSE * encoder2.getPosition();
RoyvZ 5:3562c205d001 292 ref_pos1 = Qset1 - q1;
RoyvZ 5:3562c205d001 293 //if (ref_pos1 < 0.1) { ref_pos1 = 0; }
RoyvZ 5:3562c205d001 294 ref_pos2 = Qset2 - q2; // Don’t use magic numbers!
RoyvZ 5:3562c205d001 295 //if (ref_pos2 < 0.1) { ref_pos2 = 0; }
RoyvZ 5:3562c205d001 296 motor1 = PI(ref_pos1, M1_KP, M1_KI, M1_TS, m1_err_int );
RoyvZ 5:3562c205d001 297 motor2 = PI(ref_pos2, M1_KP, M1_KI, M1_TS, m2_err_int );
RoyvZ 5:3562c205d001 298 if (ref_pos1 <= 0) {
RoyvZ 5:3562c205d001 299 motorD1=1;
RoyvZ 5:3562c205d001 300 }
RoyvZ 5:3562c205d001 301 else{
RoyvZ 5:3562c205d001 302 motorD1 = 0;
RoyvZ 5:3562c205d001 303 }
RoyvZ 5:3562c205d001 304 if (ref_pos2 <= 0) {
RoyvZ 5:3562c205d001 305 motorD2=0;
RoyvZ 5:3562c205d001 306 }
RoyvZ 5:3562c205d001 307 else{
RoyvZ 5:3562c205d001 308 motorD2=1;
RoyvZ 5:3562c205d001 309 }
RoyvZ 5:3562c205d001 310 UpperMotorDir.write(motorD1);
RoyvZ 5:3562c205d001 311 UpperMotorVel.write(motor1);
RoyvZ 5:3562c205d001 312 LowerMotorDir.write(motorD2);
RoyvZ 5:3562c205d001 313 LowerMotorVel.write(motor2);
RoyvZ 5:3562c205d001 314 }
RoyvZ 5:3562c205d001 315
RoyvZ 5:3562c205d001 316 /** Servo motor control function **/
RoyvZ 5:3562c205d001 317
RoyvZ 5:3562c205d001 318 //Servo initialisation
RoyvZ 5:3562c205d001 319 enum servostates{Close, Open};
RoyvZ 5:3562c205d001 320
RoyvZ 5:3562c205d001 321 //Set default state to open, as mentioned in paragraph ... of the report
RoyvZ 5:3562c205d001 322 servostates CurrentState = Open;
RoyvZ 5:3562c205d001 323
RoyvZ 5:3562c205d001 324 int i = 0;
RoyvZ 5:3562c205d001 325 int j = 0;
RoyvZ 5:3562c205d001 326
RoyvZ 5:3562c205d001 327 //Simply switch the current state when button is pressed
RoyvZ 5:3562c205d001 328 void Change() {
RoyvZ 5:3562c205d001 329 if(CurrentState == Close){
RoyvZ 5:3562c205d001 330 CurrentState = Open;
RoyvZ 5:3562c205d001 331 }
RoyvZ 5:3562c205d001 332 else{
RoyvZ 5:3562c205d001 333 CurrentState = Close;
RoyvZ 5:3562c205d001 334 }
DiondeGreef 4:afa85283eb18 335 }
DiondeGreef 4:afa85283eb18 336
RoyvZ 5:3562c205d001 337 //State machine for the servo motors, to determine the state
RoyvZ 5:3562c205d001 338 void ProcessStateMachine(void) {
RoyvZ 5:3562c205d001 339 /**
RoyvZ 5:3562c205d001 340 State machine for the servo motors, motor response dependent on the current state and the amount of milliseconds of activation.
RoyvZ 5:3562c205d001 341 The entire function must be sampled at 2000 Hz, since it counts half milliseconds. This is required since the servos respond to half millisecond actuation.
RoyvZ 5:3562c205d001 342 For further information on the functioning of the servo motors; see paragraph ... in report 'Design of a Jenga playing robot' by group 2.
RoyvZ 5:3562c205d001 343 **/
RoyvZ 5:3562c205d001 344 switch (CurrentState)
RoyvZ 5:3562c205d001 345 {
RoyvZ 5:3562c205d001 346 case Close:
RoyvZ 5:3562c205d001 347 if(j <= 35){
RoyvZ 5:3562c205d001 348 RightServoVel.write(0);
RoyvZ 5:3562c205d001 349 LeftServoVel.write(0);
RoyvZ 5:3562c205d001 350 j++;
RoyvZ 5:3562c205d001 351 }
RoyvZ 5:3562c205d001 352 else if (j == 36){
RoyvZ 5:3562c205d001 353 RightServoVel.write(1);
RoyvZ 5:3562c205d001 354 LeftServoVel.write(0);
RoyvZ 5:3562c205d001 355 j++;
RoyvZ 5:3562c205d001 356 }
RoyvZ 5:3562c205d001 357 else if (j == 37){
RoyvZ 5:3562c205d001 358 RightServoVel.write(1);
RoyvZ 5:3562c205d001 359 LeftServoVel.write(0);
RoyvZ 5:3562c205d001 360 j++;
RoyvZ 5:3562c205d001 361 }
RoyvZ 5:3562c205d001 362 else if (j == 38){
RoyvZ 5:3562c205d001 363 RightServoVel.write(1);
RoyvZ 5:3562c205d001 364 LeftServoVel.write(0);
RoyvZ 5:3562c205d001 365 j++;
RoyvZ 5:3562c205d001 366 }
RoyvZ 5:3562c205d001 367 else if (j == 39){
RoyvZ 5:3562c205d001 368 RightServoVel.write(1);
RoyvZ 5:3562c205d001 369 LeftServoVel.write(1);
RoyvZ 5:3562c205d001 370 j = 0;
RoyvZ 5:3562c205d001 371 }
RoyvZ 5:3562c205d001 372 break;
RoyvZ 5:3562c205d001 373
RoyvZ 5:3562c205d001 374 case Open:
RoyvZ 5:3562c205d001 375 if(j <= 35){
RoyvZ 5:3562c205d001 376 RightServoVel.write(0);
RoyvZ 5:3562c205d001 377 LeftServoVel.write(0);
RoyvZ 5:3562c205d001 378 j++;
RoyvZ 5:3562c205d001 379 }
RoyvZ 5:3562c205d001 380 else if (j == 36){
RoyvZ 5:3562c205d001 381 RightServoVel.write(0);
RoyvZ 5:3562c205d001 382 LeftServoVel.write(0);
RoyvZ 5:3562c205d001 383 j++;
RoyvZ 5:3562c205d001 384 }
RoyvZ 5:3562c205d001 385 else if (j == 37){
RoyvZ 5:3562c205d001 386 RightServoVel.write(0);
RoyvZ 5:3562c205d001 387 LeftServoVel.write(1);
RoyvZ 5:3562c205d001 388 j++;
RoyvZ 5:3562c205d001 389 }
RoyvZ 5:3562c205d001 390 else if (j == 38){
RoyvZ 5:3562c205d001 391 RightServoVel.write(1);
RoyvZ 5:3562c205d001 392 LeftServoVel.write(1);
RoyvZ 5:3562c205d001 393 j++;
RoyvZ 5:3562c205d001 394 }
RoyvZ 5:3562c205d001 395 else if (j == 39){
RoyvZ 5:3562c205d001 396 RightServoVel.write(1);
RoyvZ 5:3562c205d001 397 LeftServoVel.write(1);
RoyvZ 5:3562c205d001 398 j = 0;
RoyvZ 5:3562c205d001 399 }
RoyvZ 5:3562c205d001 400 break;
RoyvZ 5:3562c205d001 401 }
RoyvZ 5:3562c205d001 402 }
RoyvZ 5:3562c205d001 403
RoyvZ 5:3562c205d001 404 //StateMachine for the moving of the end effector
RoyvZ 5:3562c205d001 405
RoyvZ 5:3562c205d001 406 void ProcessStateMachineMain(void) {
RoyvZ 5:3562c205d001 407 states CurrentState1 = calibEMG;
RoyvZ 5:3562c205d001 408 switch (CurrentState1)
RoyvZ 5:3562c205d001 409 {
RoyvZ 5:3562c205d001 410 case MotionEndEffector:
RoyvZ 5:3562c205d001 411 m1_Ticker.attach( &PositionControl, M1_TS );
RoyvZ 5:3562c205d001 412 setpoint_Ticker.attach(&qSetpointSet, Periode);
RoyvZ 5:3562c205d001 413 break;
RoyvZ 5:3562c205d001 414
RoyvZ 5:3562c205d001 415 case calibEMG:
RoyvZ 5:3562c205d001 416 //Function to calibrate the filtered EMG signals to the user, in order to control the motors
RoyvZ 5:3562c205d001 417 double maxsignal = 0; //empty variable to check the maximal EMG output
RoyvZ 5:3562c205d001 418 for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 5 seconds
RoyvZ 5:3562c205d001 419 double signalfinal = fabs(bqc.step(emg0.read())); //get values from the filtered EMG signal
RoyvZ 5:3562c205d001 420 if (signalfinal >= maxsignal){ //check whether the received signal is higher or lower than the current highest signal value
RoyvZ 5:3562c205d001 421 maxsignal = signalfinal; //keep changing the maximal signal
RoyvZ 5:3562c205d001 422 }
RoyvZ 5:3562c205d001 423 }
RoyvZ 5:3562c205d001 424 break;
RoyvZ 5:3562c205d001 425
RoyvZ 5:3562c205d001 426 case calib1:
RoyvZ 5:3562c205d001 427 UpperMotorVel = Cal_Strength; UpperMotorDir = true; //depends on definitions
RoyvZ 5:3562c205d001 428 LowerMotorVel = 0.0f;
RoyvZ 5:3562c205d001 429 UpperMotorPos = encoder1.getPosition();
RoyvZ 5:3562c205d001 430 if (abs(UpperMotorPos - UpperMotorPos_prev) < 0.01f){
RoyvZ 5:3562c205d001 431 if (Cal_Strength < 1.0f) {
RoyvZ 5:3562c205d001 432 Cal_Strength += 0.05f;
RoyvZ 5:3562c205d001 433 }
RoyvZ 5:3562c205d001 434 else{
RoyvZ 5:3562c205d001 435 encoder1.setPosition(0); //Afhankelijk van RKI uitzoeken of deze echt naar nul moet of naar 0.5pi
RoyvZ 5:3562c205d001 436 CurrentState1 = calib2;
RoyvZ 5:3562c205d001 437 Cal_Strength = 0.6f;
RoyvZ 5:3562c205d001 438 }
RoyvZ 5:3562c205d001 439 int UpperMotorPos_prev = UpperMotorPos;
RoyvZ 5:3562c205d001 440 }
RoyvZ 5:3562c205d001 441 break;
RoyvZ 5:3562c205d001 442
RoyvZ 5:3562c205d001 443 case calib2:
RoyvZ 5:3562c205d001 444 LowerMotorVel = Cal_Strength; LowerMotorDir = true; // Check definitions
RoyvZ 5:3562c205d001 445 UpperMotorVel = 1.0f;
RoyvZ 5:3562c205d001 446 LowerMotorPos = encoder2.getPosition();
RoyvZ 5:3562c205d001 447 if (abs(LowerMotorPos - UpperMotorPos_prev) < 0.01f){
RoyvZ 5:3562c205d001 448 if (Cal_Strength < 1.0f) {
RoyvZ 5:3562c205d001 449 Cal_Strength += 0.05f;
RoyvZ 5:3562c205d001 450 }
RoyvZ 5:3562c205d001 451 else{
RoyvZ 5:3562c205d001 452 encoder2.setPosition(0);
RoyvZ 5:3562c205d001 453 CurrentState1 = Idle;
RoyvZ 5:3562c205d001 454 //M1_controller = true; //turn on the position controller here
RoyvZ 5:3562c205d001 455 UpperMotorVel = 0.0f; LowerMotorVel = 0.0f;
RoyvZ 5:3562c205d001 456 }
RoyvZ 5:3562c205d001 457 int UpperMotorPos_prev = UpperMotorPos;
RoyvZ 5:3562c205d001 458 }
RoyvZ 5:3562c205d001 459 break;
RoyvZ 5:3562c205d001 460
RoyvZ 5:3562c205d001 461 case Idle:
RoyvZ 5:3562c205d001 462 emgX = 0;
RoyvZ 5:3562c205d001 463 emgY = 0;
RoyvZ 5:3562c205d001 464 m1_Ticker.attach( &PositionControl, M1_TS );
RoyvZ 5:3562c205d001 465 setpoint_Ticker.attach(&qSetpointSet, Periode);
RoyvZ 5:3562c205d001 466 break;
RoyvZ 5:3562c205d001 467
RoyvZ 5:3562c205d001 468 case Off:
RoyvZ 5:3562c205d001 469 //Robot is off, also return to default state so you have a reference position when it starts up again
RoyvZ 5:3562c205d001 470 break;
RoyvZ 5:3562c205d001 471
RoyvZ 5:3562c205d001 472 default:
RoyvZ 5:3562c205d001 473 //Robot is in idle mode, not doing anything (motors still powered to hold current position) (Posible when waiting for your turn)
RoyvZ 5:3562c205d001 474
RoyvZ 5:3562c205d001 475 }
RoyvZ 5:3562c205d001 476 }
RoyvZ 5:3562c205d001 477
RoyvZ 5:3562c205d001 478 //Main function for the servo motors, to change open or closed state
RoyvZ 5:3562c205d001 479 int main() {
RoyvZ 5:3562c205d001 480 //allow for the 2000 Hz sampling of the Servo state machine function
RoyvZ 5:3562c205d001 481 MainTicker.attach(ProcessStateMachineMain, 0.0005);
RoyvZ 5:3562c205d001 482
RoyvZ 5:3562c205d001 483 ServoTick.attach(ProcessStateMachine, 0.0005);
RoyvZ 5:3562c205d001 484
RoyvZ 5:3562c205d001 485
RoyvZ 5:3562c205d001 486
RoyvZ 5:3562c205d001 487
RoyvZ 5:3562c205d001 488
RoyvZ 5:3562c205d001 489 /** EMG signals **/
RoyvZ 2:293665548183 490 //Constructing the notch filter chain
RoyvZ 3:2ffbee47fb38 491 bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 );
RoyvZ 5:3562c205d001 492 //Sample every 0.002 seconds, or 500 Hz
RoyvZ 5:3562c205d001 493 sample_timer.attach(&sample, 0.002);
RoyvZ 5:3562c205d001 494 //Ticker to check if motor is being calibrated, also at 500 Hz
RoyvZ 5:3562c205d001 495 calX_timer.attach(&calibrationX, 0.002);
RoyvZ 5:3562c205d001 496 calY_timer.attach(&calibrationY, 0.002);
RoyvZ 5:3562c205d001 497 //Turn LED off, turned on when calibrating
RoyvZ 5:3562c205d001 498 led2 = 1;
RoyvZ 3:2ffbee47fb38 499
RoyvZ 5:3562c205d001 500 turning.rise(&NegPosChanger);
RoyvZ 5:3562c205d001 501
RoyvZ 5:3562c205d001 502 //MODSERIAL connection
RoyvZ 5:3562c205d001 503 pc.baud(115200);
RoyvZ 5:3562c205d001 504
RoyvZ 5:3562c205d001 505
RoyvZ 5:3562c205d001 506 while (true)
RoyvZ 5:3562c205d001 507 {
DiondeGreef 4:afa85283eb18 508
RoyvZ 5:3562c205d001 509 if (buttonCal.read() == false){ //when calibrate button is pressed
RoyvZ 5:3562c205d001 510 states CurrentState1 = calibEMG;
RoyvZ 5:3562c205d001 511 }
RoyvZ 5:3562c205d001 512
RoyvZ 5:3562c205d001 513 if (!ServoButton) {
RoyvZ 5:3562c205d001 514 Change();
RoyvZ 5:3562c205d001 515 wait(0.3);
RoyvZ 5:3562c205d001 516 }
RoyvZ 5:3562c205d001 517
RoyvZ 5:3562c205d001 518
RoyvZ 5:3562c205d001 519 /*Control end-effector
RoyvZ 5:3562c205d001 520 wait(0.1);
RoyvZ 5:3562c205d001 521 float v_ref = GetReferenceVelocity();
RoyvZ 5:3562c205d001 522 setMotor(v_ref);
RoyvZ 5:3562c205d001 523 pc.printf("%f \r\n", FeedForwardControl(v_ref));
RoyvZ 5:3562c205d001 524 UpperMotorDir.write(motorDirection);
RoyvZ 5:3562c205d001 525 UpperMotorVel.write(RightServoVel); //PWM Speed Control
RoyvZ 5:3562c205d001 526 */
RoyvZ 0:b6c8d56842ce 527 }
RoyvZ 5:3562c205d001 528 }