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@6:643c44fb044a, 2017-11-03 (annotated)

Committer:
RoyvZ
Date:
Fri Nov 03 10:56:44 2017 +0000
Revision:
6:643c44fb044a
Parent:
5:3562c205d001 
Tried

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