Koen Lotze / Includes_kinematic_model

kinematic model included

Dependencies:   Encoder MODSERIAL biquadFilter

f8f7934e8460/main.cpp@0:ef7fd98bf091, 2017-10-26 (annotated)

Committer:
Tetragonia
Date:
Thu Oct 26 14:20:40 2017 +0000
Revision:
0:ef7fd98bf091
Implementation kinematic model

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tetragonia 0:ef7fd98bf091 1 #include "mbed.h"
Tetragonia 0:ef7fd98bf091 2 #include "encoder.h"
Tetragonia 0:ef7fd98bf091 3 #include "MODSERIAL.h"
Tetragonia 0:ef7fd98bf091 4 #include "BiQuad.h"
Tetragonia 0:ef7fd98bf091 5 #include "math.h"
Tetragonia 0:ef7fd98bf091 6
Tetragonia 0:ef7fd98bf091 7 #define M_Pi 3.141592653589793238462643383279502884L
Tetragonia 0:ef7fd98bf091 8
Tetragonia 0:ef7fd98bf091 9 Serial pc(USBTX, USBRX);
Tetragonia 0:ef7fd98bf091 10
Tetragonia 0:ef7fd98bf091 11 DigitalOut led_red(LED_RED);
Tetragonia 0:ef7fd98bf091 12 DigitalOut led_blue(LED_BLUE);
Tetragonia 0:ef7fd98bf091 13 InterruptIn button1(D2);
Tetragonia 0:ef7fd98bf091 14 InterruptIn button2(D3);
Tetragonia 0:ef7fd98bf091 15 AnalogIn EMG_A0(A0);
Tetragonia 0:ef7fd98bf091 16 AnalogIn EMG_A1(A1);
Tetragonia 0:ef7fd98bf091 17
Tetragonia 0:ef7fd98bf091 18 DigitalOut motor1DirectionPin(D4);
Tetragonia 0:ef7fd98bf091 19 PwmOut motor1MagnitudePin(D5);
Tetragonia 0:ef7fd98bf091 20 DigitalOut motor2DirectionPin(D7); // Sequence? Lines 181-183
Tetragonia 0:ef7fd98bf091 21 PwmOut motor2MagnitudePin(D6);
Tetragonia 0:ef7fd98bf091 22
Tetragonia 0:ef7fd98bf091 23 Ticker measureTick;
Tetragonia 0:ef7fd98bf091 24
Tetragonia 0:ef7fd98bf091 25 Encoder motor1(D13,D12); //On the shield actually M2
Tetragonia 0:ef7fd98bf091 26 Encoder motor2(D11,D10); //On the shield actually M1 (Production mistake?)
Tetragonia 0:ef7fd98bf091 27
Tetragonia 0:ef7fd98bf091 28 int Count1 = 0;
Tetragonia 0:ef7fd98bf091 29 int Count2 = 0;
Tetragonia 0:ef7fd98bf091 30 int MaxEMGcount = 1800; // Split up for motor 1 and 2 AND create a MinEMGCount
Tetragonia 0:ef7fd98bf091 31 int MinEMGcount = -1800;
Tetragonia 0:ef7fd98bf091 32 bool switch1 = 0; // manual switch for when to start calculations (later removed for a state machine
Tetragonia 0:ef7fd98bf091 33 bool direction1 = 1;
Tetragonia 0:ef7fd98bf091 34 bool direction2 = 1;
Tetragonia 0:ef7fd98bf091 35 const double RAD_PER_PULSE = 0.002991; // Value for RAD_PER_PULSE given through the slides (wrong?)
Tetragonia 0:ef7fd98bf091 36 const double DEG_PER_RAD = 180 / M_Pi; // Basic knowledge of how many degrees are in 1 radian.
Tetragonia 0:ef7fd98bf091 37 int MaxAveragerValues = 500;
Tetragonia 0:ef7fd98bf091 38 double Av_A0 = 0;
Tetragonia 0:ef7fd98bf091 39 double Av_A1 = 0;
Tetragonia 0:ef7fd98bf091 40 double q1 = 0; // Angle of arm 1 (upper) in starting position is 0 degrees
Tetragonia 0:ef7fd98bf091 41 double q2 = 179/DEG_PER_RAD; // Angle of arm 2 (lower) in starting position is 180 degrees (but can't be 0 or 180 because of determinant = 0)
Tetragonia 0:ef7fd98bf091 42 int L1 = 47; // Length of arm 1 (upper) in cm
Tetragonia 0:ef7fd98bf091 43 int L2 = 29; // Length of arm 2 (lower) in cm
Tetragonia 0:ef7fd98bf091 44 int xdes = L1-L2 // Desired x coordinate, arm is located at x = L1-L2 in starting position
Tetragonia 0:ef7fd98bf091 45 int ydes = 0; // Disired y coordinate, arm is located at y = 0 in starting position
Tetragonia 0:ef7fd98bf091 46
Tetragonia 0:ef7fd98bf091 47 // Sample time (motor1-timestep)
Tetragonia 0:ef7fd98bf091 48 const double M1_Ts = 0.01; //100 Hz systems
Tetragonia 0:ef7fd98bf091 49 const double M2_Ts = 0.01;
Tetragonia 0:ef7fd98bf091 50
Tetragonia 0:ef7fd98bf091 51 // Controller gains (motor1-Kp,-Ki,...)
Tetragonia 0:ef7fd98bf091 52 const double M1_Kp = 0.1, M1_Ki = 0.02, M1_Kd = 0.00125, M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT
Tetragonia 0:ef7fd98bf091 53 const double M2_Kp = 0.1, M2_Ki = 0.02, M2_Kd = 0.00125, M2_N = 100; // Inspired by the Ziegler-Nichols Method
Tetragonia 0:ef7fd98bf091 54
Tetragonia 0:ef7fd98bf091 55 // Filter variables (motor1-filter-v1,-v2)
Tetragonia 0:ef7fd98bf091 56 double M1_f_v1 = 0.0, M1_f_v2 = 0.0;
Tetragonia 0:ef7fd98bf091 57 double M2_f_v1 = 0.0, M2_f_v2 = 0.0;
Tetragonia 0:ef7fd98bf091 58
Tetragonia 0:ef7fd98bf091 59 // PROGRAM THAT CALCULATES ANGLE CHANGES
Tetragonia 0:ef7fd98bf091 60
Tetragonia 0:ef7fd98bf091 61
Tetragonia 0:ef7fd98bf091 62 // PROGRAM THAT ANALYSES EMG SIGNAL AND FILTERS IT
Tetragonia 0:ef7fd98bf091 63 // Define the HIDScope and Ticker object
Tetragonia 0:ef7fd98bf091 64 //HIDScope scope(2);
Tetragonia 0:ef7fd98bf091 65 //Ticker scopeTimer;
Tetragonia 0:ef7fd98bf091 66
Tetragonia 0:ef7fd98bf091 67 BiQuadChain bqchain1, bqchain2; //BiQuad Chain. Makes a combination of various BiQuads.
Tetragonia 0:ef7fd98bf091 68
Tetragonia 0:ef7fd98bf091 69 BiQuad bq1(0.960582636413903, -1.921165272827806, 0.960582636413903, -1.916303032724640, 0.926027512930974);//Highpass 8Hz
Tetragonia 0:ef7fd98bf091 70 BiQuad bq2(0.912835456766912, -1.825670913533825, 0.912835456766912, -1.821050358261190, 0.830291468806461);//
Tetragonia 0:ef7fd98bf091 71 BiQuad bq3(0.991153589776121, -1.603846829332603, 0.991153589776121, -1.593257429417890, 0.982171881701343);//Notch 49-51Hz
Tetragonia 0:ef7fd98bf091 72 BiQuad bq4(0.991153589776121, -1.603846829332603, 0.991153589776121, -1.614311809680850, 0.982599066293185);//
Tetragonia 0:ef7fd98bf091 73 BiQuad bq5(0.064591432448980, 0.129182864897960, 0.064591432448980, -1.401313635492332, 0.659679365288253);//Lowpasss 45Hz
Tetragonia 0:ef7fd98bf091 74 BiQuad bq6(0.052062866562661, 0.104125733125322, 0.052062866562661, -1.129505912021716, 0.337757378272360);//
Tetragonia 0:ef7fd98bf091 75
Tetragonia 0:ef7fd98bf091 76 BiQuadChain bqchain3, bqchain4; //BiQuad Chain. Makes a combination of various BiQuads.
Tetragonia 0:ef7fd98bf091 77
Tetragonia 0:ef7fd98bf091 78 BiQuad bq7(0.960582636413903, -1.921165272827806, 0.960582636413903, -1.916303032724640, 0.926027512930974);//Highpass 8Hz
Tetragonia 0:ef7fd98bf091 79 BiQuad bq8(0.912835456766912, -1.825670913533825, 0.912835456766912, -1.821050358261190, 0.830291468806461);//
Tetragonia 0:ef7fd98bf091 80 BiQuad bq9(0.991153589776121, -1.603846829332603, 0.991153589776121, -1.593257429417890, 0.982171881701343);//Notch 49-51Hz
Tetragonia 0:ef7fd98bf091 81 BiQuad bq10(0.991153589776121, -1.603846829332603, 0.991153589776121, -1.614311809680850, 0.982599066293185);//
Tetragonia 0:ef7fd98bf091 82 BiQuad bq11(0.064591432448980, 0.129182864897960, 0.064591432448980, -1.401313635492332, 0.659679365288253);//Lowpasss 45Hz
Tetragonia 0:ef7fd98bf091 83 BiQuad bq12(0.052062866562661, 0.104125733125322, 0.052062866562661, -1.129505912021716, 0.337757378272360);//
Tetragonia 0:ef7fd98bf091 84
Tetragonia 0:ef7fd98bf091 85 double A0t10=0, A0t9=0, A0t8=0, A0t7=0, A0t6=0, A0t5=0, A0t4=0, A0t3=0, A0t2=0, A0t1=0;
Tetragonia 0:ef7fd98bf091 86 double A1t10=0, A1t9=0, A1t8=0, A1t7=0, A1t6=0, A1t5=0, A1t4=0, A1t3=0, A1t2=0, A1t1=0;
Tetragonia 0:ef7fd98bf091 87
Tetragonia 0:ef7fd98bf091 88 double EMGSample1(double Input, double &t10, double &t9, double &t8, double &t7, double &t6, double &t5, double &t4, double &t3, double &t2, double &t1){ // Sample function: read a value from the Analog in
Tetragonia 0:ef7fd98bf091 89 //double MaxSignal = 0;
Tetragonia 0:ef7fd98bf091 90 double Signal = 0.0f;
Tetragonia 0:ef7fd98bf091 91 double EMGFiltered = fabs(bqchain1.step(Input)); // Signal.Read instantly gets filtered.
Tetragonia 0:ef7fd98bf091 92 double EMGSignal = bqchain2.step(EMGFiltered);
Tetragonia 0:ef7fd98bf091 93 double treshhold = 0.6*Av_A0;
Tetragonia 0:ef7fd98bf091 94 // if (EMGSignal > MaxSignal){ MaxSignal = EMGSignal;} // Save the highest value measured.
Tetragonia 0:ef7fd98bf091 95 // double discrete = floor((EMGSignal/MaxSignal)*10.0)/10.0;
Tetragonia 0:ef7fd98bf091 96 if (EMGSignal >= treshhold || t1 >= treshhold || t2 >= treshhold || t3 >= treshhold || t4 >= treshhold || t5 >= treshhold || t6 >= treshhold || t7 >= treshhold || t8 >= treshhold || t9 >= treshhold || t10 >= treshhold ){
Tetragonia 0:ef7fd98bf091 97 Signal = 1;
Tetragonia 0:ef7fd98bf091 98 }
Tetragonia 0:ef7fd98bf091 99 else {
Tetragonia 0:ef7fd98bf091 100 Signal = 0.0f;
Tetragonia 0:ef7fd98bf091 101 }
Tetragonia 0:ef7fd98bf091 102 t10=t9, t9=t8, t8=t7, t7=t6, t6=t5, t5=t4, t4=t3, t3=t2, t2=t1, t1= EMGSignal;
Tetragonia 0:ef7fd98bf091 103
Tetragonia 0:ef7fd98bf091 104 return Signal;
Tetragonia 0:ef7fd98bf091 105 }
Tetragonia 0:ef7fd98bf091 106
Tetragonia 0:ef7fd98bf091 107 double EMGSample2(double Input, double &t10, double &t9, double &t8, double &t7, double &t6, double &t5, double &t4, double &t3, double &t2, double &t1){ // Sample function: read a value from the Analog in
Tetragonia 0:ef7fd98bf091 108 //double MaxSignal = 0;
Tetragonia 0:ef7fd98bf091 109 double Signal = 0.0f;
Tetragonia 0:ef7fd98bf091 110 double EMGFiltered = fabs(bqchain3.step(Input)); // Signal.Read instantly gets filtered.
Tetragonia 0:ef7fd98bf091 111 double EMGSignal = bqchain4.step(EMGFiltered);
Tetragonia 0:ef7fd98bf091 112 double treshhold = 0.6*Av_A1;
Tetragonia 0:ef7fd98bf091 113 // if (EMGSignal > MaxSignal){ MaxSignal = EMGSignal;} // Save the highest value measured.
Tetragonia 0:ef7fd98bf091 114 // double discrete = floor((EMGSignal/MaxSignal)*10.0)/10.0;
Tetragonia 0:ef7fd98bf091 115 if (EMGSignal >= treshhold || t1 >= treshhold || t2 >= treshhold || t3 >= treshhold || t4 >= treshhold || t5 >= treshhold || t6 >= treshhold || t7 >= treshhold || t8 >= treshhold || t9 >= treshhold || t10 >= treshhold ){
Tetragonia 0:ef7fd98bf091 116 Signal = 1;
Tetragonia 0:ef7fd98bf091 117 }
Tetragonia 0:ef7fd98bf091 118 else {
Tetragonia 0:ef7fd98bf091 119 Signal = 0.0f;
Tetragonia 0:ef7fd98bf091 120 }
Tetragonia 0:ef7fd98bf091 121 t10=t9, t9=t8, t8=t7, t7=t6, t6=t5, t5=t4, t4=t3, t3=t2, t2=t1, t1= EMGSignal;
Tetragonia 0:ef7fd98bf091 122
Tetragonia 0:ef7fd98bf091 123 return Signal;
Tetragonia 0:ef7fd98bf091 124 }
Tetragonia 0:ef7fd98bf091 125
Tetragonia 0:ef7fd98bf091 126 // PROGRAM THAT MEASURES AN AMOUNT OF VALUES AND EXPORTS THE AVERAGE
Tetragonia 0:ef7fd98bf091 127 void AverageValue_A0(double &Av_A0)
Tetragonia 0:ef7fd98bf091 128 {
Tetragonia 0:ef7fd98bf091 129 double A[MaxAveragerValues];
Tetragonia 0:ef7fd98bf091 130 double sum = 0;
Tetragonia 0:ef7fd98bf091 131 for( int i = 0; i < MaxAveragerValues; i++){
Tetragonia 0:ef7fd98bf091 132 double EMGFiltered = fabs(bqchain1.step(EMG_A0.read())); // Signal.Read instantly gets filtered.
Tetragonia 0:ef7fd98bf091 133 double Value = bqchain2.step(EMGFiltered);
Tetragonia 0:ef7fd98bf091 134 wait(0.01);
Tetragonia 0:ef7fd98bf091 135 for (int j = 1; j < MaxAveragerValues; j++){
Tetragonia 0:ef7fd98bf091 136 A[MaxAveragerValues-j] = A[MaxAveragerValues-1-j];
Tetragonia 0:ef7fd98bf091 137 }
Tetragonia 0:ef7fd98bf091 138 A[0] = Value;
Tetragonia 0:ef7fd98bf091 139 }
Tetragonia 0:ef7fd98bf091 140 for (int i = 0; i < MaxAveragerValues; i++){
Tetragonia 0:ef7fd98bf091 141 sum = sum + A[i];
Tetragonia 0:ef7fd98bf091 142 }
Tetragonia 0:ef7fd98bf091 143 printf("result = %.0f", (sum/MaxAveragerValues));
Tetragonia 0:ef7fd98bf091 144 Av_A0 = (sum/MaxAveragerValues);
Tetragonia 0:ef7fd98bf091 145 }
Tetragonia 0:ef7fd98bf091 146
Tetragonia 0:ef7fd98bf091 147 void AverageValue_A1(double &Av_A1)
Tetragonia 0:ef7fd98bf091 148 {
Tetragonia 0:ef7fd98bf091 149 double A[MaxAveragerValues];
Tetragonia 0:ef7fd98bf091 150 double sum = 0;
Tetragonia 0:ef7fd98bf091 151 for( int i = 0; i < MaxAveragerValues; i++){
Tetragonia 0:ef7fd98bf091 152 double EMGFiltered = fabs(bqchain3.step(EMG_A1.read()));
Tetragonia 0:ef7fd98bf091 153 double Value = bqchain4.step(EMGFiltered);
Tetragonia 0:ef7fd98bf091 154 wait(0.01);
Tetragonia 0:ef7fd98bf091 155 for (int j = 1; j < MaxAveragerValues; j++){
Tetragonia 0:ef7fd98bf091 156 A[MaxAveragerValues-j] = A[MaxAveragerValues-1-j];
Tetragonia 0:ef7fd98bf091 157 }
Tetragonia 0:ef7fd98bf091 158 A[0] = Value;
Tetragonia 0:ef7fd98bf091 159 }
Tetragonia 0:ef7fd98bf091 160 for (int i = 0; i < MaxAveragerValues; i++){
Tetragonia 0:ef7fd98bf091 161 sum = sum + A[i];
Tetragonia 0:ef7fd98bf091 162 }
Tetragonia 0:ef7fd98bf091 163 printf("result = %.0f", (sum/MaxAveragerValues));
Tetragonia 0:ef7fd98bf091 164 Av_A1 = (sum/MaxAveragerValues);
Tetragonia 0:ef7fd98bf091 165 }
Tetragonia 0:ef7fd98bf091 166
Tetragonia 0:ef7fd98bf091 167 // PROGRAM THAT ENABLES THE SYSTEM TO OPERATE (Later to be replaced by State Machine)
Tetragonia 0:ef7fd98bf091 168 void but1_pressed(void)
Tetragonia 0:ef7fd98bf091 169 {
Tetragonia 0:ef7fd98bf091 170 if (switch1 == 0){
Tetragonia 0:ef7fd98bf091 171 pc.printf("TAKING AVERAGES...");
Tetragonia 0:ef7fd98bf091 172 for(int i = 1; i < 12; i++){
Tetragonia 0:ef7fd98bf091 173 led_blue = !led_blue;
Tetragonia 0:ef7fd98bf091 174 wait(0.1);
Tetragonia 0:ef7fd98bf091 175 }
Tetragonia 0:ef7fd98bf091 176 AverageValue_A0(Av_A0);
Tetragonia 0:ef7fd98bf091 177 pc.printf("Av_A0 = %.3f ", Av_A0);
Tetragonia 0:ef7fd98bf091 178 led_blue = 1;
Tetragonia 0:ef7fd98bf091 179 for(int i = 1; i < 12; i++){
Tetragonia 0:ef7fd98bf091 180 led_blue = !led_blue;
Tetragonia 0:ef7fd98bf091 181 wait(0.1);
Tetragonia 0:ef7fd98bf091 182 }
Tetragonia 0:ef7fd98bf091 183 AverageValue_A1(Av_A1);
Tetragonia 0:ef7fd98bf091 184 pc.printf("Av_A1 = %.3f \r\n\n", Av_A1);
Tetragonia 0:ef7fd98bf091 185 led_blue = 1;
Tetragonia 0:ef7fd98bf091 186 }
Tetragonia 0:ef7fd98bf091 187 wait(10.0);
Tetragonia 0:ef7fd98bf091 188 led_red = !led_red;
Tetragonia 0:ef7fd98bf091 189 switch1 = !switch1;
Tetragonia 0:ef7fd98bf091 190 }
Tetragonia 0:ef7fd98bf091 191
Tetragonia 0:ef7fd98bf091 192 void but2_pressed(void)
Tetragonia 0:ef7fd98bf091 193 {
Tetragonia 0:ef7fd98bf091 194 direction1 = !direction1;
Tetragonia 0:ef7fd98bf091 195 direction2 = !direction2;
Tetragonia 0:ef7fd98bf091 196 }
Tetragonia 0:ef7fd98bf091 197
Tetragonia 0:ef7fd98bf091 198 // PROGRAM THAT CALCULATES THE PID
Tetragonia 0:ef7fd98bf091 199 double PID( double err, const double Kp, const double Ki, const double Kd,
Tetragonia 0:ef7fd98bf091 200 const double Ts, const double N, double &v1, double &v2 ) {
Tetragonia 0:ef7fd98bf091 201
Tetragonia 0:ef7fd98bf091 202 const double a1 = -4/(N*Ts+2), a2 = -(N*Ts-2)/(N*Ts+2), // a1 and a2 are the nominator of our transferfunction
Tetragonia 0:ef7fd98bf091 203 b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
Tetragonia 0:ef7fd98bf091 204 b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2),
Tetragonia 0:ef7fd98bf091 205 b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4); // b0, b1 and b2 the denominator
Tetragonia 0:ef7fd98bf091 206
Tetragonia 0:ef7fd98bf091 207 double v = err - a1*v1 - a2*v2; // Memory value are calculated and later on stored. (v is like an input)
Tetragonia 0:ef7fd98bf091 208 double u = b0*v + b1*v1 + b2*v2;
Tetragonia 0:ef7fd98bf091 209 v2 = v1; v1 = v;
Tetragonia 0:ef7fd98bf091 210 return u; // u functions as our output value gained from the transferfunction.
Tetragonia 0:ef7fd98bf091 211 }
Tetragonia 0:ef7fd98bf091 212
Tetragonia 0:ef7fd98bf091 213 // PROGRAM THAT RECEIVES EMG SIGNALS AND CONVERTS THEM TO REFERENCE POSITIONS
Tetragonia 0:ef7fd98bf091 214 double EMG_Referencer(double sig, bool dir, int &n) {
Tetragonia 0:ef7fd98bf091 215 //pc.printf("MEASURING");
Tetragonia 0:ef7fd98bf091 216 if (dir == 1){
Tetragonia 0:ef7fd98bf091 217 if (sig > 0.99){ //Above 0.9 because of inconsistency in received signals
Tetragonia 0:ef7fd98bf091 218 if (n < MaxEMGcount){
Tetragonia 0:ef7fd98bf091 219 n = n++; //Normally in steps of 1 (so n = n++;) but for better visibility we have 36 itterations
Tetragonia 0:ef7fd98bf091 220 }
Tetragonia 0:ef7fd98bf091 221 }
Tetragonia 0:ef7fd98bf091 222 }
Tetragonia 0:ef7fd98bf091 223 else if (dir == 0){
Tetragonia 0:ef7fd98bf091 224 if (sig > 0.99){
Tetragonia 0:ef7fd98bf091 225 if (n > MinEMGcount){ //MinEMGcount
Tetragonia 0:ef7fd98bf091 226 n = n--; // Normally n = n--;
Tetragonia 0:ef7fd98bf091 227 }
Tetragonia 0:ef7fd98bf091 228 }
Tetragonia 0:ef7fd98bf091 229 }
Tetragonia 0:ef7fd98bf091 230 double u = 0.1*n;
Tetragonia 0:ef7fd98bf091 231 return u;
Tetragonia 0:ef7fd98bf091 232 }
Tetragonia 0:ef7fd98bf091 233
Tetragonia 0:ef7fd98bf091 234 //Kinematic model
Tetragonia 0:ef7fd98bf091 235 double Kinematic_referencer(double xdes, bool ydes, int &n) { //weet niet wat ik met die n moet
Tetragonia 0:ef7fd98bf091 236 //printf ("x=%f, y=%f\n", x, y);
Tetragonia 0:ef7fd98bf091 237 double qt = q1 + q2; // current total angle
Tetragonia 0:ef7fd98bf091 238 double xcurrent = L1 * cos (q1) + L2 * cos (qt); // current x position
Tetragonia 0:ef7fd98bf091 239 double ycurrent = L1 * sin (q1) + L2 * sin (qt); // current y position
Tetragonia 0:ef7fd98bf091 240
Tetragonia 0:ef7fd98bf091 241 //Initial twist
Tetragonia 0:ef7fd98bf091 242 double vx = (xdes - xcurrent)/100; // Running on 100 Hz
Tetragonia 0:ef7fd98bf091 243 double vy = (ydes - ycurrent)/100;
Tetragonia 0:ef7fd98bf091 244
Tetragonia 0:ef7fd98bf091 245 //Jacobians
Tetragonia 0:ef7fd98bf091 246 double J11 = -ycurrent;
Tetragonia 0:ef7fd98bf091 247 double J12 = -L2 * sin (qt);
Tetragonia 0:ef7fd98bf091 248 double J21 = xcurrent;
Tetragonia 0:ef7fd98bf091 249 double J22 = L2 * cos (qt);
Tetragonia 0:ef7fd98bf091 250 double Determinant = J11 * J22 - J21 * J12; // calculate determinant
Tetragonia 0:ef7fd98bf091 251
Tetragonia 0:ef7fd98bf091 252 //Calculate angular velocities
Tetragonia 0:ef7fd98bf091 253 double q1der = (J22 * vx - J12 * vy) / Determinant;
Tetragonia 0:ef7fd98bf091 254 double q2der = (-J21 * vx + J11 * vy) / Determinant;
Tetragonia 0:ef7fd98bf091 255
Tetragonia 0:ef7fd98bf091 256 //Calculate new angles
Tetragonia 0:ef7fd98bf091 257 q1new = q1 + q1der/100; //nog fixen met die tijdstappen?
Tetragonia 0:ef7fd98bf091 258 q2new = q2 + q2der/100; //hier ook
Tetragonia 0:ef7fd98bf091 259 //printf ("q1=%f, q2=%f\n", q1 * c, q2 * c);
Tetragonia 0:ef7fd98bf091 260 qtnew = q1new + q2new;
Tetragonia 0:ef7fd98bf091 261
Tetragonia 0:ef7fd98bf091 262 //Calculate new positions
Tetragonia 0:ef7fd98bf091 263 xnew = L1 * cos (q1new) + L2 * cos (qtnew);
Tetragonia 0:ef7fd98bf091 264 ynew = L1 * sin (q1new) + L2 * sin (qtnew);
Tetragonia 0:ef7fd98bf091 265 //printf ("x=%f, y=%f\n", x, y);
Tetragonia 0:ef7fd98bf091 266
Tetragonia 0:ef7fd98bf091 267 // Now check whether the calculated position is desired, determinants close to zero may cause the robot to move weirdly
Tetragonia 0:ef7fd98bf091 268 // New y may not be negative, this means the arm is located in on the plate
Tetragonia 0:ef7fd98bf091 269 // New q1 may not be less than -45 degrees, less means the arm will crash into the base plate
Tetragonia 0:ef7fd98bf091 270 // New q2 may not be more than 185 degrees, more means the lower arm will crash into the upper arm
Tetragonia 0:ef7fd98bf091 271 if (ynew > -1 && q1new > -45 / DEG_PER_RAD && q2new < 185 / DEG_PER_RAD)
Tetragonia 0:ef7fd98bf091 272 {
Tetragonia 0:ef7fd98bf091 273 // If desired, change the angles
Tetragonia 0:ef7fd98bf091 274 q1 = q1new;
Tetragonia 0:ef7fd98bf091 275 q2 = q2new;
Tetragonia 0:ef7fd98bf091 276 }
Tetragonia 0:ef7fd98bf091 277 else
Tetragonia 0:ef7fd98bf091 278 {
Tetragonia 0:ef7fd98bf091 279 // If not desired, don't change the angles, but define current position as desired so the robot ignores the input
Tetragonia 0:ef7fd98bf091 280 xdes = xcurrent;
Tetragonia 0:ef7fd98bf091 281 ydes = ycurrent;
Tetragonia 0:ef7fd98bf091 282 }
Tetragonia 0:ef7fd98bf091 283 return q1,q2;
Tetragonia 0:ef7fd98bf091 284 }
Tetragonia 0:ef7fd98bf091 285
Tetragonia 0:ef7fd98bf091 286 // PROGRAMS THAT CONTROL THE VALUE OUTPUT
Tetragonia 0:ef7fd98bf091 287 double M1_Controller(double Angle1) {
Tetragonia 0:ef7fd98bf091 288 double EMG_Input_A0 = EMG_A0.read();
Tetragonia 0:ef7fd98bf091 289 double sig1 = EMGSample1(EMG_Input_A0, A0t10, A0t9, A0t8, A0t7, A0t6, A0t5, A0t4, A0t3, A0t2, A0t1);
Tetragonia 0:ef7fd98bf091 290
Tetragonia 0:ef7fd98bf091 291 double referencePosition = EMG_Referencer(sig1, direction1, Count1);
Tetragonia 0:ef7fd98bf091 292 pc.printf("D1 = %i, S1 = %.0f ", Count1, sig1);
Tetragonia 0:ef7fd98bf091 293 double motor1Value = PID( referencePosition - Angle1 , M1_Kp, M1_Ki, M1_Kd, M1_Ts, M1_N, M1_f_v1, M1_f_v2); //Find the motorvalue by going through the PID
Tetragonia 0:ef7fd98bf091 294
Tetragonia 0:ef7fd98bf091 295 return motor1Value; // According to the PID its calculations, we get a motor value.
Tetragonia 0:ef7fd98bf091 296 }
Tetragonia 0:ef7fd98bf091 297
Tetragonia 0:ef7fd98bf091 298 double M2_Controller(double Angle2) {
Tetragonia 0:ef7fd98bf091 299 double EMG_Input_A1 = EMG_A1.read();; //EMG_A1.read();
Tetragonia 0:ef7fd98bf091 300 double sig2 = EMGSample2(EMG_Input_A1, A1t10, A1t9, A1t8, A1t7, A1t6, A1t5, A1t4, A1t3, A1t2, A1t1);
Tetragonia 0:ef7fd98bf091 301
Tetragonia 0:ef7fd98bf091 302 double referencePosition = EMG_Referencer(sig2, direction2, Count2);; // Since we have 360 degrees, our potmeter should reach these values as well.
Tetragonia 0:ef7fd98bf091 303 pc.printf("D2 = %i, S2 = %.0f \r\n", Count2, sig2);
Tetragonia 0:ef7fd98bf091 304 double motor2Value = PID( referencePosition - Angle2 , M2_Kp, M2_Ki, M2_Kd, M2_Ts, M2_N, M2_f_v1, M2_f_v2); //Find the motorvalue by going through the PID
Tetragonia 0:ef7fd98bf091 305
Tetragonia 0:ef7fd98bf091 306 return motor2Value; // According to the PID its calculations, we get a motor value.
Tetragonia 0:ef7fd98bf091 307 }
Tetragonia 0:ef7fd98bf091 308
Tetragonia 0:ef7fd98bf091 309 // PROGRAMS FOR POWERING THE MOTOR ACCORDING TO THE ERROR (P VARIANT)
Tetragonia 0:ef7fd98bf091 310 void SetMotor1(double motor1Value) // function that actually changes the output for the motor
Tetragonia 0:ef7fd98bf091 311 {
Tetragonia 0:ef7fd98bf091 312 if(motor1Value >= 0 ) //Function sets direction and strength
Tetragonia 0:ef7fd98bf091 313 motor1DirectionPin = 1; //If the reference value is positive, we will turn clockwise
Tetragonia 0:ef7fd98bf091 314 else
Tetragonia 0:ef7fd98bf091 315 motor1DirectionPin = 0; // if not, counterclockwise
Tetragonia 0:ef7fd98bf091 316
Tetragonia 0:ef7fd98bf091 317 if(fabs(motor1Value) > 0.2 ) // Next, check the absolute motor value, which is the magnitude
Tetragonia 0:ef7fd98bf091 318 motor1MagnitudePin = 0.2; // This is a safety. We never want to exceed 1
Tetragonia 0:ef7fd98bf091 319 else
Tetragonia 0:ef7fd98bf091 320 motor1MagnitudePin = fabs(motor1Value); // if we fall underneath the safety, take the magnitude
Tetragonia 0:ef7fd98bf091 321 }
Tetragonia 0:ef7fd98bf091 322
Tetragonia 0:ef7fd98bf091 323 void SetMotor2(double motor2Value) // function that actually changes the output for the motor
Tetragonia 0:ef7fd98bf091 324 {
Tetragonia 0:ef7fd98bf091 325 if(motor2Value >= 0 ) //Function sets direction and strength
Tetragonia 0:ef7fd98bf091 326 motor2DirectionPin = 0; //If the reference value is positive, we will turn clockwise
Tetragonia 0:ef7fd98bf091 327 else
Tetragonia 0:ef7fd98bf091 328 motor2DirectionPin = 1; // if not, counterclockwise
Tetragonia 0:ef7fd98bf091 329
Tetragonia 0:ef7fd98bf091 330 if(fabs(motor2Value) > 0.2 ) // Next, check the absolute motor value, which is the magnitude
Tetragonia 0:ef7fd98bf091 331 motor2MagnitudePin = 0.2; // This is a safety. We never want to exceed 1
Tetragonia 0:ef7fd98bf091 332 else
Tetragonia 0:ef7fd98bf091 333 motor2MagnitudePin = fabs(motor2Value); // if we fall underneath the safety, take the magnitude
Tetragonia 0:ef7fd98bf091 334 }
Tetragonia 0:ef7fd98bf091 335
Tetragonia 0:ef7fd98bf091 336 // PROGRAM THAT MEASURES AND CONTROLES THE MOTOR OUTPUT
Tetragonia 0:ef7fd98bf091 337 void MeasureAndControl() // Pure values being calculated and send to the Mbed.
Tetragonia 0:ef7fd98bf091 338 {
Tetragonia 0:ef7fd98bf091 339 double Angle1 = DEG_PER_RAD * RAD_PER_PULSE * motor1.getPosition(); // Angle is equal to the degrees per pulse measured (NOT TRUE AT THIS MOMENT, '360' != 2 * M_Pi
Tetragonia 0:ef7fd98bf091 340 double Angle2 = DEG_PER_RAD * RAD_PER_PULSE * motor2.getPosition(); // Angle is equal to the degrees per pulse measured (NOT TRUE AT THIS MOMENT, '360' != 2 * M_Pi
Tetragonia 0:ef7fd98bf091 341
Tetragonia 0:ef7fd98bf091 342 if (switch1 == 1){ // If the switch is pressed
Tetragonia 0:ef7fd98bf091 343 double motor1Value = M1_Controller(Angle1); // create a motorvalue to give to motor 1 according to the control and angle
Tetragonia 0:ef7fd98bf091 344 double motor2Value = M2_Controller(Angle2); // create a motorvalue to give to motor 1 according to the control and angle
Tetragonia 0:ef7fd98bf091 345
Tetragonia 0:ef7fd98bf091 346 SetMotor1(motor1Value);
Tetragonia 0:ef7fd98bf091 347 SetMotor2(motor2Value);
Tetragonia 0:ef7fd98bf091 348 }
Tetragonia 0:ef7fd98bf091 349 else {
Tetragonia 0:ef7fd98bf091 350 SetMotor1(0);
Tetragonia 0:ef7fd98bf091 351 SetMotor2(0); }
Tetragonia 0:ef7fd98bf091 352 }
Tetragonia 0:ef7fd98bf091 353
Tetragonia 0:ef7fd98bf091 354 int main() // Main function
Tetragonia 0:ef7fd98bf091 355 {
Tetragonia 0:ef7fd98bf091 356 pc.baud(115200); // For post analysis, seeing if the plug works etc.
Tetragonia 0:ef7fd98bf091 357 pc.printf("STARTING SEQUENCE \r\n"); //Merely checking if there is a serial connection at all
Tetragonia 0:ef7fd98bf091 358 bqchain1.add(&bq1).add(&bq2).add(&bq3).add(&bq4); // Combine the BiQuads bq1, bq2 and bq3 in the Chain.
Tetragonia 0:ef7fd98bf091 359 bqchain2.add(&bq5).add(&bq6);
Tetragonia 0:ef7fd98bf091 360 bqchain3.add(&bq7).add(&bq8).add(&bq9).add(&bq10); // Combine the BiQuads bq1, bq2 and bq3 in the Chain.
Tetragonia 0:ef7fd98bf091 361 bqchain4.add(&bq11).add(&bq12);
Tetragonia 0:ef7fd98bf091 362 measureTick.attach(&MeasureAndControl, M1_Ts); // Tick that changes the motor (currently 1Hz)
Tetragonia 0:ef7fd98bf091 363 led_red = 1; // Set the LED off in the positive direction, on in the negative direction
Tetragonia 0:ef7fd98bf091 364 led_blue = 1;
Tetragonia 0:ef7fd98bf091 365 button1.fall(&but1_pressed); // Trigger the InterruptIn of the button.
Tetragonia 0:ef7fd98bf091 366 button2.fall(&but2_pressed);
Tetragonia 0:ef7fd98bf091 367 //scopeTimer.attach_us(&scopeSend, 1e4); // Attach in Microseconds
Tetragonia 0:ef7fd98bf091 368 }
Tetragonia 0:ef7fd98bf091 369