Group 9 BioRobotics / Mbed 2 deprecated the_code

The code without functions and variables filled in

Dependencies:   HIDScope Servo mbed QEI biquadFilter

THE.cpp@16:37b491eac34b, 2018-11-01 (annotated)

Committer:
s1725696
Date:
Thu Nov 01 11:14:44 2018 +0000
Revision:
16:37b491eac34b
Parent:
15:2772f8cbf382
Child:
17:65943f6e11dc
RKI and PID control implemented

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1725696 5:3581013d4505 1 #include "mbed.h" // Use revision 119!!
s1725696 5:3581013d4505 2 #include "HIDScope.h" // For displaying data, select MBED - HID device, restart for every new code
s1725696 5:3581013d4505 3 #include "QEI.h" // For reading the encoder of the motors
s1725696 9:d7a6a3619576 4 #include <ctime> // for the timer during the process (if needed)
s1725696 11:79311abb2bc2 5 #include "Servo.h" // For controlling the servo
s1725696 13:c087c0f64769 6 #include "BiQuad.h"
s1725696 0:cb8857cf3ea4 7
s1725696 5:3581013d4505 8 #define SERIAL_BAUD 115200
s1725696 5:3581013d4505 9
s1725696 5:3581013d4505 10 // In- en outputs
s1725696 4:8183e7b228f0 11 // -----------------------------------------------------------------------------
s1725696 4:8183e7b228f0 12
s1725696 0:cb8857cf3ea4 13 // EMG related
s1725696 13:c087c0f64769 14 // Constants EMG filter
s1725696 13:c087c0f64769 15 const double m1 = 0.5000;
s1725696 13:c087c0f64769 16 const double m2 = -0.8090;
s1725696 13:c087c0f64769 17 const double n0 = 0.5000;
s1725696 13:c087c0f64769 18 const double n1 = -0.8090;
s1725696 13:c087c0f64769 19 const double n2 = 0.0;
s1725696 13:c087c0f64769 20 const double a1 = 0.9565;
s1725696 13:c087c0f64769 21 const double a2 = -1.9131;
s1725696 13:c087c0f64769 22 const double b0 = 0.9565;
s1725696 13:c087c0f64769 23 const double b1 = -1.9112;
s1725696 13:c087c0f64769 24 const double b2 = 0.9150;
s1725696 13:c087c0f64769 25 const double c1 = 0.0675;
s1725696 13:c087c0f64769 26 const double c2 = 0.1349;
s1725696 13:c087c0f64769 27 const double d0 = 0.0675;
s1725696 13:c087c0f64769 28 const double d1 = -1.1430;
s1725696 13:c087c0f64769 29 const double d2 = 0.4128;
s1725696 13:c087c0f64769 30
s1725696 13:c087c0f64769 31 // Variables EMG
s1725696 13:c087c0f64769 32 double emg0;
s1725696 13:c087c0f64769 33 double emg1;
s1725696 13:c087c0f64769 34 double emg2;
s1725696 13:c087c0f64769 35 double notch0;
s1725696 13:c087c0f64769 36 double notch1;
s1725696 13:c087c0f64769 37 double notch2;
s1725696 13:c087c0f64769 38 double high0;
s1725696 13:c087c0f64769 39 double high1;
s1725696 13:c087c0f64769 40 double high2;
s1725696 13:c087c0f64769 41 double absolute0;
s1725696 13:c087c0f64769 42 double absolute1;
s1725696 13:c087c0f64769 43 double absolute2;
s1725696 13:c087c0f64769 44 double low0;
s1725696 13:c087c0f64769 45 double low1;
s1725696 13:c087c0f64769 46 double low2;
s1725696 13:c087c0f64769 47
s1725696 13:c087c0f64769 48 // BiQuad values
s1725696 13:c087c0f64769 49 BiQuadChain notch;
s1725696 13:c087c0f64769 50 BiQuad N1( m1, m2, n0, n1, n2);
s1725696 13:c087c0f64769 51 BiQuad N2( m1, m2, n0, n1, n2);
s1725696 13:c087c0f64769 52 BiQuad N3( m1, m2, n0, n1, n2);
s1725696 13:c087c0f64769 53 BiQuadChain highpass;
s1725696 13:c087c0f64769 54 BiQuad H1( a1, a2, b0, b1, b2);
s1725696 13:c087c0f64769 55 BiQuad H2( a1, a2, b0, b1, b2);
s1725696 13:c087c0f64769 56 BiQuad H3( a1, a2, b0, b1, b2);
s1725696 13:c087c0f64769 57 BiQuadChain lowpass;
s1725696 13:c087c0f64769 58 BiQuad L1( c1, c2, d0, d1, d2);
s1725696 13:c087c0f64769 59 BiQuad L2( c1, c2, d0, d1, d2);
s1725696 13:c087c0f64769 60 BiQuad L3( c1, c2, d0, d1, d2);
s1725696 13:c087c0f64769 61
s1725696 16:37b491eac34b 62 const float T = 0.001f;
s1725696 13:c087c0f64769 63
s1725696 13:c087c0f64769 64 // EMG
s1725696 13:c087c0f64769 65 const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated
s1725696 13:c087c0f64769 66 double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array
s1725696 13:c087c0f64769 67 double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {};
s1725696 13:c087c0f64769 68
s1725696 13:c087c0f64769 69 //Empty arrays to calculate MovAgs
s1725696 13:c087c0f64769 70 double Average0, Average1, Average2; //Outcome of MovAg
s1725696 13:c087c0f64769 71 const int sizeCali = 500; //Size of array over which the Threshold will be calculated
s1725696 13:c087c0f64769 72 double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {};
s1725696 13:c087c0f64769 73
s1725696 13:c087c0f64769 74 //Empty arrays to calculate means in calibration
s1725696 13:c087c0f64769 75 double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration
s1725696 13:c087c0f64769 76 double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2
s1725696 13:c087c0f64769 77 int g = 0; //Part of the switch void, where the current state can be changed
s1725696 13:c087c0f64769 78 int emg_calib=0; //After calibration this value will be 1, enabling the
s1725696 13:c087c0f64769 79
s1725696 13:c087c0f64769 80 // EMG
s1725696 13:c087c0f64769 81 Ticker Filter_tick;
s1725696 13:c087c0f64769 82 Ticker MovAg_tick;
s1725696 13:c087c0f64769 83 AnalogIn emg0_in( A0 );
s1725696 13:c087c0f64769 84 AnalogIn emg1_in( A1 );
s1725696 13:c087c0f64769 85 AnalogIn emg2_in( A2 );
s1725696 10:56136a0da8c1 86
s1725696 0:cb8857cf3ea4 87 // Motor related
s1725696 12:b2b082e73ef1 88 DigitalOut dirpin_1(D4); // direction of motor 1 (translation)
s1725696 0:cb8857cf3ea4 89 PwmOut pwmpin_1(D5); // PWM pin of motor 1
s1725696 12:b2b082e73ef1 90 DigitalOut dirpin_2(D7); // direction of motor 2 (rotation)
s1725696 5:3581013d4505 91 PwmOut pwmpin_2(D6); // PWM pin of motor 2
s1725696 0:cb8857cf3ea4 92
s1725696 16:37b491eac34b 93 // RKI related
s1725696 16:37b491eac34b 94 const double Ts = 0.001;// sample frequency
s1725696 16:37b491eac34b 95
s1725696 16:37b491eac34b 96 // Constants motor
s1725696 16:37b491eac34b 97 const double delta_t = 0.01;
s1725696 16:37b491eac34b 98 const double L1 = 370.0 / 2.0;
s1725696 16:37b491eac34b 99 const double L2 = 65.0 / 2.0;
s1725696 16:37b491eac34b 100 const double pi = 3.14159265359;
s1725696 16:37b491eac34b 101 const double alpha = (2.0 * pi) /(25.0*8400.0);
s1725696 16:37b491eac34b 102 const double beta = (((2.0 * L1) - (2.0 * L2)) * 20.0 * pi) / (305.0 * 8400.0);
s1725696 16:37b491eac34b 103 const double q1start = rotation_end_position * alpha;
s1725696 16:37b491eac34b 104 const double q2start = tower_1_position * beta;
s1725696 16:37b491eac34b 105 const double q2end = tower_end_position * beta;
s1725696 16:37b491eac34b 106
s1725696 16:37b491eac34b 107 // Variables motors
s1725696 16:37b491eac34b 108 double out1;
s1725696 16:37b491eac34b 109 double out2;
s1725696 16:37b491eac34b 110 int counts1;
s1725696 16:37b491eac34b 111 int counts2;
s1725696 16:37b491eac34b 112 double vdesx;
s1725696 16:37b491eac34b 113 double vdesy;
s1725696 16:37b491eac34b 114 double q1;
s1725696 16:37b491eac34b 115 double q2;
s1725696 16:37b491eac34b 116 double MPe;
s1725696 16:37b491eac34b 117 double xe;
s1725696 16:37b491eac34b 118 double ye;
s1725696 16:37b491eac34b 119 double gamma;
s1725696 16:37b491eac34b 120 double dq1;
s1725696 16:37b491eac34b 121 double dq2;
s1725696 16:37b491eac34b 122 double dC1;
s1725696 16:37b491eac34b 123 double dC2;
s1725696 16:37b491eac34b 124 double pwm1;
s1725696 16:37b491eac34b 125 double pwm2;
s1725696 16:37b491eac34b 126
s1725696 16:37b491eac34b 127 // PID rotation constants
s1725696 16:37b491eac34b 128 double Rot_Kp = 1.5;
s1725696 16:37b491eac34b 129 double Rot_Ki = 0.1;
s1725696 16:37b491eac34b 130 double Rot_Kd = 0.48;
s1725696 16:37b491eac34b 131 double Rot_error = 0;
s1725696 16:37b491eac34b 132 double Rot_prev_error = 0;
s1725696 16:37b491eac34b 133
s1725696 16:37b491eac34b 134 // PID translation constants
s1725696 16:37b491eac34b 135 const double Trans_Kp = 0.5;
s1725696 16:37b491eac34b 136 const double Trans_Ki = 0.5;
s1725696 16:37b491eac34b 137 const double Trans_Kd = 0.1;
s1725696 16:37b491eac34b 138 double Trans_error = 0;
s1725696 16:37b491eac34b 139 double Trans_prev_error = 0;
s1725696 16:37b491eac34b 140
s1725696 0:cb8857cf3ea4 141 // Extra stuff
s1725696 9:d7a6a3619576 142 DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
s1725696 8:5ad8a7892693 143 DigitalIn button_emergency(D7); // button for emergency mode, on bioshield
s1725696 9:d7a6a3619576 144 DigitalIn button_wait(SW3); // button for wait mode, on mbed
s1725696 8:5ad8a7892693 145 DigitalIn button_demo(D6); // button for demo mode, on bioshield
s1725696 9:d7a6a3619576 146
s1725696 4:8183e7b228f0 147 DigitalIn led_red(LED_RED); // red led
s1725696 4:8183e7b228f0 148 DigitalIn led_green(LED_GREEN); // green led
s1725696 4:8183e7b228f0 149 DigitalIn led_blue(LED_BLUE); // blue led
s1725696 9:d7a6a3619576 150
s1725696 5:3581013d4505 151 AnalogIn pot_1(A1); // potmeter for simulating EMG input
s1725696 13:c087c0f64769 152 Servo myservo(D8); // Define the servo to control (in penholder), up to start with
s1725696 0:cb8857cf3ea4 153
s1725696 0:cb8857cf3ea4 154 // Other stuff
s1725696 4:8183e7b228f0 155 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 156 // Define stuff like tickers etc
s1725696 12:b2b082e73ef1 157
s1725696 10:56136a0da8c1 158 Ticker ticker; // Name a ticker, use each ticker only for 1 function!
s1725696 13:c087c0f64769 159 HIDScope scope(6); // Number of channels in HIDScope
s1725696 12:b2b082e73ef1 160 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2)
s1725696 12:b2b082e73ef1 161 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
s1725696 5:3581013d4505 162 Serial pc(USBTX,USBRX);
s1725696 6:f495a77c2c95 163 Timer t; // For timing the time in each state (https://os.mbed.com/handbook/Timer)
s1725696 0:cb8857cf3ea4 164
s1725696 0:cb8857cf3ea4 165 // Variables
s1725696 4:8183e7b228f0 166 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 167 // Define here all variables needed throughout the whole code
s1725696 12:b2b082e73ef1 168 volatile double time_overall;
s1725696 12:b2b082e73ef1 169 volatile double time_in_state;
s1725696 12:b2b082e73ef1 170 volatile double motor_velocity = 0;
s1725696 12:b2b082e73ef1 171 volatile double EMG = 0;
s1725696 12:b2b082e73ef1 172 volatile double errors = 0;
s1725696 12:b2b082e73ef1 173 volatile int counts1_prev = 0;
s1725696 12:b2b082e73ef1 174 volatile int counts2_prev = 0;
s1725696 12:b2b082e73ef1 175 volatile int counts1;
s1725696 12:b2b082e73ef1 176 volatile int counts2;
s1725696 12:b2b082e73ef1 177
s1725696 12:b2b082e73ef1 178 // MOTOR_CAL
s1725696 12:b2b082e73ef1 179 volatile double tower_1_position = 0.1; // the tower which he reaches first
s1725696 12:b2b082e73ef1 180 volatile double tower_end_position = 0.1; // the tower which he reaches second
s1725696 12:b2b082e73ef1 181 volatile double rotation_start_position = 0.1; // the position where the rotation will remain
s1725696 12:b2b082e73ef1 182 volatile double position;
s1725696 12:b2b082e73ef1 183 float speed = 0.70;
s1725696 12:b2b082e73ef1 184 int dir = 0;
s1725696 6:f495a77c2c95 185
s1725696 6:f495a77c2c95 186 // states
s1725696 9:d7a6a3619576 187 enum states {WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO}; // states the robot can be in
s1725696 7:ec5add330cb3 188 states CurrentState = WAIT; // the CurrentState to start with is the WAIT state
s1725696 6:f495a77c2c95 189 bool StateChanged = true; // the state must be changed to go into the next state
s1725696 0:cb8857cf3ea4 190
s1725696 0:cb8857cf3ea4 191 // Functions
s1725696 4:8183e7b228f0 192 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 193
s1725696 4:8183e7b228f0 194 // Encoder
s1725696 4:8183e7b228f0 195 // Getting encoder information from motors
s1725696 12:b2b082e73ef1 196 int Counts1(volatile int& a) // a = counts1
s1725696 4:8183e7b228f0 197 {
s1725696 12:b2b082e73ef1 198 counts1_prev = a;
s1725696 12:b2b082e73ef1 199 a = Encoder2.getPulses();
s1725696 12:b2b082e73ef1 200 return a;
s1725696 12:b2b082e73ef1 201 }
s1725696 12:b2b082e73ef1 202
s1725696 12:b2b082e73ef1 203 int Counts2(volatile int& a) // a = counts2
s1725696 12:b2b082e73ef1 204 {
s1725696 12:b2b082e73ef1 205 counts2_prev = a;
s1725696 12:b2b082e73ef1 206 a = Encoder1.getPulses();
s1725696 12:b2b082e73ef1 207 return a;
s1725696 12:b2b082e73ef1 208 }
s1725696 5:3581013d4505 209
s1725696 14:abc125dcc246 210 // Potmeter for controlling motor, must be removed when EMG is used to control the motor
s1725696 5:3581013d4505 211 float potmeter()
s1725696 5:3581013d4505 212 {
s1725696 14:abc125dcc246 213 float out_2 = (pot_1 * 2.0f) - 1.0f;
s1725696 5:3581013d4505 214
s1725696 5:3581013d4505 215 dirpin_1.write(out_2 < 0);
s1725696 5:3581013d4505 216 dirpin_2.write(out_2 < 0);
s1725696 5:3581013d4505 217
s1725696 5:3581013d4505 218 pwmpin_1 = fabs (out_2); // Has to be positive value
s1725696 5:3581013d4505 219 pwmpin_2 = fabs (out_2);
s1725696 5:3581013d4505 220
s1725696 5:3581013d4505 221 return out_2;
s1725696 6:f495a77c2c95 222 }
s1725696 9:d7a6a3619576 223
s1725696 11:79311abb2bc2 224 // Servo control
s1725696 12:b2b082e73ef1 225 // To lift the pen up, with a push of button
s1725696 11:79311abb2bc2 226 void servocontrol()
s1725696 11:79311abb2bc2 227 {
s1725696 14:abc125dcc246 228 while (button_motorcal == false) // If button is pushed, pen should go up
s1725696 11:79311abb2bc2 229 {
s1725696 11:79311abb2bc2 230 myservo = 0.1;
s1725696 11:79311abb2bc2 231 }
s1725696 14:abc125dcc246 232 myservo = 0.0;
s1725696 6:f495a77c2c95 233 }
s1725696 9:d7a6a3619576 234
s1725696 10:56136a0da8c1 235 // EMG filter
s1725696 10:56136a0da8c1 236 // To process the EMG signal before information can be caught from it
s1725696 14:abc125dcc246 237
s1725696 14:abc125dcc246 238 // Filter of the first EMG signal
s1725696 13:c087c0f64769 239 void filtering()
s1725696 13:c087c0f64769 240 {
s1725696 13:c087c0f64769 241 // Reading the EMG signal
s1725696 13:c087c0f64769 242 emg0 = emg0_in.read();
s1725696 13:c087c0f64769 243 emg1 = emg1_in.read();
s1725696 13:c087c0f64769 244 emg2 = emg2_in.read();
s1725696 13:c087c0f64769 245
s1725696 13:c087c0f64769 246 // Applying a notch filter over the EMG data
s1725696 13:c087c0f64769 247 notch0 = N1.step(emg0);
s1725696 13:c087c0f64769 248 notch1 = N2.step(emg1);
s1725696 13:c087c0f64769 249 notch2 = N3.step(emg2);
s1725696 13:c087c0f64769 250
s1725696 13:c087c0f64769 251 // Applying a high pass filter
s1725696 13:c087c0f64769 252 high0 = H1.step(notch0);
s1725696 13:c087c0f64769 253 high1 = H2.step(notch1);
s1725696 13:c087c0f64769 254 high2 = H3.step(notch2);
s1725696 13:c087c0f64769 255
s1725696 13:c087c0f64769 256 // Rectifying the signal
s1725696 13:c087c0f64769 257 absolute0 = fabs(high0);
s1725696 13:c087c0f64769 258 absolute1 = fabs(high1);
s1725696 13:c087c0f64769 259 absolute2 = fabs(high2);
s1725696 13:c087c0f64769 260
s1725696 13:c087c0f64769 261 // Applying low pass filter
s1725696 13:c087c0f64769 262 low0 = L1.step(absolute0);
s1725696 13:c087c0f64769 263 low1 = L2.step(absolute1);
s1725696 13:c087c0f64769 264 low2 = L3.step(absolute2);
s1725696 13:c087c0f64769 265 }
s1725696 13:c087c0f64769 266
s1725696 13:c087c0f64769 267 // Moving average filter
s1725696 13:c087c0f64769 268 // To determine the moving average, apply after filtering
s1725696 13:c087c0f64769 269 void MovAg()
s1725696 13:c087c0f64769 270 {
s1725696 13:c087c0f64769 271 // For statement to make an array of the last datapoints of the filtered signal
s1725696 13:c087c0f64769 272 for (int i = sizeMovAg - 1; i >= 0; i--)
s1725696 13:c087c0f64769 273 {
s1725696 13:c087c0f64769 274 // Shifts the i'th element one place to the right
s1725696 13:c087c0f64769 275 StoreArray0[i] = StoreArray0[i-1];
s1725696 13:c087c0f64769 276 StoreArray1[i] = StoreArray1[i-1];
s1725696 13:c087c0f64769 277 StoreArray2[i] = StoreArray2[i-1];
s1725696 13:c087c0f64769 278 }
s1725696 13:c087c0f64769 279
s1725696 13:c087c0f64769 280 // Stores the latest datapoint in the first element of the array
s1725696 13:c087c0f64769 281 StoreArray0[0] = low0;
s1725696 13:c087c0f64769 282 StoreArray1[0] = low1;
s1725696 13:c087c0f64769 283 StoreArray2[0] = low2;
s1725696 13:c087c0f64769 284 sum1 = 0.0;
s1725696 13:c087c0f64769 285 sum2 = 0.0;
s1725696 13:c087c0f64769 286 sum3 = 0.0;
s1725696 13:c087c0f64769 287
s1725696 13:c087c0f64769 288 // For statement to sum the elements in the array
s1725696 13:c087c0f64769 289 for (int a = 0; a<=sizeMovAg-1; a++)
s1725696 13:c087c0f64769 290 {
s1725696 13:c087c0f64769 291 sum1+=StoreArray0[a];
s1725696 13:c087c0f64769 292 sum2+=StoreArray1[a];
s1725696 13:c087c0f64769 293 sum3+=StoreArray2[a];
s1725696 13:c087c0f64769 294 }
s1725696 13:c087c0f64769 295
s1725696 13:c087c0f64769 296 // Calculates an average over the datapoints in the array
s1725696 13:c087c0f64769 297 Average0 = sum1/sizeMovAg;
s1725696 13:c087c0f64769 298 Average1 = sum2/sizeMovAg;
s1725696 13:c087c0f64769 299 Average2 = sum3/sizeMovAg;
s1725696 13:c087c0f64769 300
s1725696 14:abc125dcc246 301 // Sending the signal to the HIDScope
s1725696 14:abc125dcc246 302 // Change the number of channels in the beginning of the script when necessary
s1725696 14:abc125dcc246 303 scope.set( 0, emg0);
s1725696 14:abc125dcc246 304 scope.set( 1, low0);
s1725696 13:c087c0f64769 305 scope.set( 2, Average0);
s1725696 13:c087c0f64769 306 scope.set( 3, low1);
s1725696 13:c087c0f64769 307 scope.set( 4, emg2);
s1725696 13:c087c0f64769 308 scope.set( 5, low2);
s1725696 13:c087c0f64769 309 scope.send();
s1725696 14:abc125dcc246 310 }
s1725696 10:56136a0da8c1 311
s1725696 10:56136a0da8c1 312 // WAIT
s1725696 10:56136a0da8c1 313 // To do nothing
s1725696 10:56136a0da8c1 314 void wait_mode()
s1725696 10:56136a0da8c1 315 {
s1725696 10:56136a0da8c1 316 // go back to the initial values
s1725696 10:56136a0da8c1 317 // Copy here the variables list with initial values
s1725696 13:c087c0f64769 318 // all pwm's to zero
s1725696 13:c087c0f64769 319 // all counts to zero
s1725696 10:56136a0da8c1 320 }
s1725696 10:56136a0da8c1 321
s1725696 10:56136a0da8c1 322 // MOTOR_CAL
s1725696 10:56136a0da8c1 323 // To calibrate the motor angle to some mechanical boundaries
s1725696 10:56136a0da8c1 324 // Kenneth mee bezig
s1725696 12:b2b082e73ef1 325 void pos_store(int a){ //store position in counts to know count location of the ends of bridge
s1725696 12:b2b082e73ef1 326
s1725696 13:c087c0f64769 327 if (tower_1_position == 0.1)
s1725696 13:c087c0f64769 328 {
s1725696 12:b2b082e73ef1 329 tower_1_position = a;
s1725696 12:b2b082e73ef1 330 }
s1725696 13:c087c0f64769 331 else if (tower_end_position == 0.1)
s1725696 13:c087c0f64769 332 {
s1725696 12:b2b082e73ef1 333 tower_end_position = a;
s1725696 12:b2b082e73ef1 334 }
s1725696 13:c087c0f64769 335 else if (rotation_start_position == 0.1)
s1725696 13:c087c0f64769 336 {
s1725696 12:b2b082e73ef1 337 rotation_start_position = a;
s1725696 12:b2b082e73ef1 338 }
s1725696 12:b2b082e73ef1 339 }
s1725696 12:b2b082e73ef1 340
s1725696 12:b2b082e73ef1 341 // Start translation
s1725696 12:b2b082e73ef1 342 void translation_start(int a, float b) // a = dir , b = speed
s1725696 12:b2b082e73ef1 343 {
s1725696 12:b2b082e73ef1 344 dirpin_1.write(a);
s1725696 12:b2b082e73ef1 345 pwmpin_1 = b;
s1725696 12:b2b082e73ef1 346 }
s1725696 12:b2b082e73ef1 347
s1725696 12:b2b082e73ef1 348 // Stop translation
s1725696 12:b2b082e73ef1 349 void translation_stop()
s1725696 12:b2b082e73ef1 350 {
s1725696 12:b2b082e73ef1 351 pwmpin_1 = 0.0;
s1725696 12:b2b082e73ef1 352 }
s1725696 12:b2b082e73ef1 353
s1725696 12:b2b082e73ef1 354 // Start rotation
s1725696 12:b2b082e73ef1 355 void rotation_start(int a, float b)
s1725696 12:b2b082e73ef1 356 {
s1725696 12:b2b082e73ef1 357 dirpin_2.write(a);
s1725696 12:b2b082e73ef1 358 pwmpin_2 = b;
s1725696 12:b2b082e73ef1 359 }
s1725696 12:b2b082e73ef1 360
s1725696 12:b2b082e73ef1 361 // Stop rotation
s1725696 12:b2b082e73ef1 362 void rotation_stop()
s1725696 12:b2b082e73ef1 363 {
s1725696 12:b2b082e73ef1 364 pwmpin_2 = 0.0;
s1725696 12:b2b082e73ef1 365 }
s1725696 12:b2b082e73ef1 366
s1725696 12:b2b082e73ef1 367 // Calibration of translation
s1725696 12:b2b082e73ef1 368 void calibration_translation()
s1725696 12:b2b082e73ef1 369 {
s1725696 12:b2b082e73ef1 370 for(int m = 1; m <= 2; m++) // to do each direction one time
s1725696 12:b2b082e73ef1 371 {
s1725696 14:abc125dcc246 372 // dir = 0, means that the pen moves to the translation motor, dir = 1 means it moves to the rotation motor
s1725696 12:b2b082e73ef1 373 pc.printf("\r\nTranslatie loop\r\n");
s1725696 12:b2b082e73ef1 374 translation_start(dir,speed);
s1725696 12:b2b082e73ef1 375 pc.printf("Direction = %i\r\n", dir);
s1725696 12:b2b082e73ef1 376
s1725696 12:b2b082e73ef1 377 bool g = true; // to make a condition for the while loop
s1725696 12:b2b082e73ef1 378 while (g == true)
s1725696 12:b2b082e73ef1 379 {
s1725696 12:b2b082e73ef1 380 if (button_demo == false) // if button_demo is pushed, the translation should stop and change direction
s1725696 12:b2b082e73ef1 381 {
s1725696 12:b2b082e73ef1 382 translation_stop();
s1725696 12:b2b082e73ef1 383 pos_store(Counts1(counts1));
s1725696 12:b2b082e73ef1 384 pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
s1725696 12:b2b082e73ef1 385 dir = dir + 1;
s1725696 12:b2b082e73ef1 386
s1725696 12:b2b082e73ef1 387 g = false; // to end the while loop
s1725696 12:b2b082e73ef1 388 }
s1725696 12:b2b082e73ef1 389
s1725696 12:b2b082e73ef1 390 wait(0.01);
s1725696 12:b2b082e73ef1 391 }
s1725696 12:b2b082e73ef1 392
s1725696 15:2772f8cbf382 393 wait(1.0); // wait 3 seconds before next round of translation/rotation
s1725696 12:b2b082e73ef1 394 }
s1725696 12:b2b082e73ef1 395 }
s1725696 12:b2b082e73ef1 396
s1725696 12:b2b082e73ef1 397 void calibration_rotation()
s1725696 12:b2b082e73ef1 398 {
s1725696 12:b2b082e73ef1 399 rotation_start(dir, speed);
s1725696 12:b2b082e73ef1 400 pc.printf("\r\nRotatie start\r\n");
s1725696 12:b2b082e73ef1 401
s1725696 12:b2b082e73ef1 402 bool f = true; // condition for while loop
s1725696 12:b2b082e73ef1 403 while(f == true)
s1725696 12:b2b082e73ef1 404 {
s1725696 12:b2b082e73ef1 405 if (button_motorcal == false) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing
s1725696 12:b2b082e73ef1 406 {
s1725696 12:b2b082e73ef1 407 rotation_stop();
s1725696 12:b2b082e73ef1 408
s1725696 12:b2b082e73ef1 409 f = false; // to end the while loop
s1725696 12:b2b082e73ef1 410 }
s1725696 12:b2b082e73ef1 411
s1725696 12:b2b082e73ef1 412 wait(0.01);
s1725696 12:b2b082e73ef1 413 }
s1725696 14:abc125dcc246 414 int start_counts = 0;
s1725696 14:abc125dcc246 415 pos_store(Counts2(start_counts));
s1725696 12:b2b082e73ef1 416 pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
s1725696 12:b2b082e73ef1 417 }
s1725696 12:b2b082e73ef1 418
s1725696 10:56136a0da8c1 419 void motor_calibration()
s1725696 10:56136a0da8c1 420 {
s1725696 12:b2b082e73ef1 421 // translation
s1725696 12:b2b082e73ef1 422 calibration_translation();
s1725696 12:b2b082e73ef1 423
s1725696 12:b2b082e73ef1 424 pc.printf("before wait\r\n");
s1725696 12:b2b082e73ef1 425 wait(1.5);
s1725696 12:b2b082e73ef1 426
s1725696 12:b2b082e73ef1 427 // rotation
s1725696 12:b2b082e73ef1 428 calibration_rotation();
s1725696 12:b2b082e73ef1 429
s1725696 12:b2b082e73ef1 430 pc.printf("Motor calibration done");
s1725696 10:56136a0da8c1 431 }
s1725696 10:56136a0da8c1 432
s1725696 10:56136a0da8c1 433 // EMG_CAL
s1725696 10:56136a0da8c1 434 // To calibrate the EMG signal to some boundary values
s1725696 13:c087c0f64769 435 // Void to switch between signals to calibrate
s1725696 13:c087c0f64769 436 void switch_to_calibrate()
s1725696 13:c087c0f64769 437 {
s1725696 13:c087c0f64769 438 g++;
s1725696 13:c087c0f64769 439 //If g = 0, led is blue
s1725696 13:c087c0f64769 440 if (g == 0)
s1725696 13:c087c0f64769 441 {
s1725696 13:c087c0f64769 442 led_blue==0;
s1725696 13:c087c0f64769 443 led_red==1;
s1725696 13:c087c0f64769 444 led_green==1;
s1725696 13:c087c0f64769 445 }
s1725696 13:c087c0f64769 446 //If g = 1, led is red
s1725696 13:c087c0f64769 447 else if(g == 1)
s1725696 13:c087c0f64769 448 {
s1725696 13:c087c0f64769 449 led_blue==1;
s1725696 13:c087c0f64769 450 led_red==0;
s1725696 13:c087c0f64769 451 led_green==1;
s1725696 13:c087c0f64769 452 }
s1725696 13:c087c0f64769 453 //If g = 2, led is green
s1725696 13:c087c0f64769 454 else if(g == 2)
s1725696 13:c087c0f64769 455 {
s1725696 13:c087c0f64769 456 led_blue==1;
s1725696 13:c087c0f64769 457 led_red==1;
s1725696 13:c087c0f64769 458 led_green==0;
s1725696 13:c087c0f64769 459 }
s1725696 13:c087c0f64769 460 //If g > 3, led is white
s1725696 13:c087c0f64769 461 else
s1725696 13:c087c0f64769 462 {
s1725696 13:c087c0f64769 463 led_blue==0;
s1725696 13:c087c0f64769 464 led_red==0;
s1725696 13:c087c0f64769 465 led_green==0;
s1725696 13:c087c0f64769 466 emg_calib = 0;
s1725696 13:c087c0f64769 467 g = 0;
s1725696 13:c087c0f64769 468 }
s1725696 14:abc125dcc246 469 }
s1725696 13:c087c0f64769 470
s1725696 13:c087c0f64769 471 // Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
s1725696 14:abc125dcc246 472 void calibrate()
s1725696 13:c087c0f64769 473 {
s1725696 13:c087c0f64769 474 switch (g)
s1725696 13:c087c0f64769 475 {
s1725696 13:c087c0f64769 476 case 0:
s1725696 13:c087c0f64769 477 { // Case zero, calibrate EMG signal of right biceps
s1725696 13:c087c0f64769 478 sum = 0.0;
s1725696 13:c087c0f64769 479
s1725696 13:c087c0f64769 480 //For statement to make an array of the latest datapoints of the filtered signal
s1725696 13:c087c0f64769 481 for (int j = 0; j<=sizeCali-1; j++)
s1725696 13:c087c0f64769 482 {
s1725696 13:c087c0f64769 483
s1725696 13:c087c0f64769 484 StoreCali0[j] = low0; // Stores the latest datapoint in the first element of the array
s1725696 13:c087c0f64769 485 sum+=StoreCali0[j]; // Sums the elements in the array
s1725696 13:c087c0f64769 486 wait(0.001f);
s1725696 13:c087c0f64769 487 }
s1725696 13:c087c0f64769 488 Mean0 = sum/sizeCali; // Calculates the mean of the signal
s1725696 13:c087c0f64769 489 Threshold0 = Mean0; // Factor *2 is for resting and *1 is for max contraction
s1725696 13:c087c0f64769 490 break;
s1725696 13:c087c0f64769 491 }
s1725696 13:c087c0f64769 492 case 1:
s1725696 13:c087c0f64769 493 { // Case one, calibrate EMG signal of left biceps
s1725696 13:c087c0f64769 494 sum = 0.0;
s1725696 13:c087c0f64769 495 for(int j=0; j<=sizeCali-1; j++)
s1725696 13:c087c0f64769 496 {
s1725696 13:c087c0f64769 497 StoreCali1[j] = low1;
s1725696 13:c087c0f64769 498 sum+=StoreCali1[j];
s1725696 13:c087c0f64769 499 wait(0.001f);
s1725696 13:c087c0f64769 500 }
s1725696 13:c087c0f64769 501 Mean1 = sum/sizeCali;
s1725696 13:c087c0f64769 502 Threshold1 = Mean1; // Factor *2 is for resting and *1 is for max contraction
s1725696 13:c087c0f64769 503 break;
s1725696 13:c087c0f64769 504 }
s1725696 13:c087c0f64769 505 case 2:
s1725696 13:c087c0f64769 506 { // Case two, calibrate EMG signal of calf
s1725696 13:c087c0f64769 507 sum = 0.0;
s1725696 13:c087c0f64769 508 for(int j=0; j<=sizeCali-1; j++)
s1725696 13:c087c0f64769 509 {
s1725696 13:c087c0f64769 510 StoreCali2[j] = low2;
s1725696 13:c087c0f64769 511 sum+=StoreCali2[j];
s1725696 13:c087c0f64769 512 wait(0.001f);
s1725696 13:c087c0f64769 513 }
s1725696 13:c087c0f64769 514 Mean2 = sum/sizeCali;
s1725696 13:c087c0f64769 515 Threshold2 = Mean2; //Factor *2 is for resting and *1 is for max contraction
s1725696 13:c087c0f64769 516 break;
s1725696 13:c087c0f64769 517 }
s1725696 13:c087c0f64769 518 case 3:
s1725696 13:c087c0f64769 519 { // Sets calibration value to 1; robot can be set to Home position
s1725696 13:c087c0f64769 520 emg_calib=1;
s1725696 13:c087c0f64769 521 wait(0.001f);
s1725696 13:c087c0f64769 522 break;
s1725696 13:c087c0f64769 523 }
s1725696 13:c087c0f64769 524 default:
s1725696 13:c087c0f64769 525 { // Ensures nothing happens if g is not equal to 0, 1 or 2.
s1725696 13:c087c0f64769 526 break;
s1725696 13:c087c0f64769 527 }
s1725696 13:c087c0f64769 528 }
s1725696 13:c087c0f64769 529 }
s1725696 13:c087c0f64769 530
s1725696 13:c087c0f64769 531 // Void to calibrate the EMG signal
s1725696 10:56136a0da8c1 532 void emg_calibration()
s1725696 10:56136a0da8c1 533 {
s1725696 13:c087c0f64769 534 for(int m = 1; m <= 4; m++)
s1725696 13:c087c0f64769 535 {
s1725696 13:c087c0f64769 536 led_blue == 0;
s1725696 13:c087c0f64769 537 led_red == 1;
s1725696 13:c087c0f64769 538 led_green == 1;
s1725696 13:c087c0f64769 539
s1725696 13:c087c0f64769 540 MovAg_tick.attach(&MovAg,T); // Moving average calculation every T sec.
s1725696 13:c087c0f64769 541
s1725696 13:c087c0f64769 542
s1725696 13:c087c0f64769 543 pc.printf("g is %i\n\r",g);
s1725696 13:c087c0f64769 544 pc.printf("Average0 = %f , Average1 = %f, Average2 = %f \n\r",Average0, Average1, Average2);
s1725696 13:c087c0f64769 545 pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
s1725696 13:c087c0f64769 546
s1725696 13:c087c0f64769 547 bool k = true;
s1725696 13:c087c0f64769 548 while(k == true)
s1725696 13:c087c0f64769 549 {
s1725696 13:c087c0f64769 550 if(button_motorcal == false)
s1725696 13:c087c0f64769 551 {
s1725696 13:c087c0f64769 552 calibrate(); // Calibrate threshold for 3 muscles
s1725696 13:c087c0f64769 553 k = false;
s1725696 13:c087c0f64769 554 }
s1725696 13:c087c0f64769 555 wait(0.2f); // Wait to avoid bouncing of button
s1725696 13:c087c0f64769 556 }
s1725696 13:c087c0f64769 557
s1725696 13:c087c0f64769 558 bool h = true;
s1725696 13:c087c0f64769 559 while(h == true)
s1725696 13:c087c0f64769 560 {
s1725696 13:c087c0f64769 561 if (button_demo == false)
s1725696 13:c087c0f64769 562 {
s1725696 13:c087c0f64769 563 switch_to_calibrate(); // Switch state of calibration (which muscle)
s1725696 13:c087c0f64769 564 h = false;
s1725696 13:c087c0f64769 565 }
s1725696 13:c087c0f64769 566
s1725696 13:c087c0f64769 567 wait(0.2f); // Wait to avoid bouncing of button
s1725696 13:c087c0f64769 568 }
s1725696 13:c087c0f64769 569 }
s1725696 14:abc125dcc246 570
s1725696 14:abc125dcc246 571 // Turning all leds off
s1725696 14:abc125dcc246 572 led_red == 1;
s1725696 14:abc125dcc246 573 led_blue == 1;
s1725696 14:abc125dcc246 574 led_green == 1;
s1725696 10:56136a0da8c1 575 }
s1725696 10:56136a0da8c1 576
s1725696 9:d7a6a3619576 577 // PID controller
s1725696 9:d7a6a3619576 578 // To control the input signal before it goes into the motor control
s1725696 9:d7a6a3619576 579 // Simon mee bezig
s1725696 16:37b491eac34b 580 // PID execution
s1725696 16:37b491eac34b 581 double PID_control(double error, const double kp, const double ki, const double kd, double &error_int, double &error_prev)
s1725696 16:37b491eac34b 582 {
s1725696 16:37b491eac34b 583 // P control
s1725696 16:37b491eac34b 584 double u_k = kp * error;
s1725696 16:37b491eac34b 585
s1725696 16:37b491eac34b 586 // I control
s1725696 16:37b491eac34b 587 error_int = error_int + (Ts * error);
s1725696 16:37b491eac34b 588 double u_i = ki * error_int;
s1725696 16:37b491eac34b 589
s1725696 16:37b491eac34b 590 // D control
s1725696 16:37b491eac34b 591 double error_deriv = (error - error_prev);
s1725696 16:37b491eac34b 592 double u_d = kd * error_deriv;
s1725696 16:37b491eac34b 593 error_prev = error;
s1725696 16:37b491eac34b 594
s1725696 16:37b491eac34b 595 return u_k + u_i + u_d;
s1725696 10:56136a0da8c1 596 }
s1725696 0:cb8857cf3ea4 597
s1725696 10:56136a0da8c1 598 // START
s1725696 0:cb8857cf3ea4 599 // To move the robot to the starting position: middle
s1725696 10:56136a0da8c1 600 void start_mode()
s1725696 4:8183e7b228f0 601 {
s1725696 12:b2b082e73ef1 602 // move to middle, only motor1 has to do something, the other one you can move yourself during the calibration
s1725696 14:abc125dcc246 603
s1725696 14:abc125dcc246 604 //translation home
s1725696 14:abc125dcc246 605 if (counts2 > ((tower_end_position - tower_1_position)/2))
s1725696 14:abc125dcc246 606 {
s1725696 14:abc125dcc246 607 translation_start(0,1.0);
s1725696 14:abc125dcc246 608 }
s1725696 14:abc125dcc246 609 else {
s1725696 14:abc125dcc246 610 translation_start(1,1.0);
s1725696 14:abc125dcc246 611 }
s1725696 14:abc125dcc246 612 if (counts2 > ((tower_end_position - tower_1_position)/2 - 100))
s1725696 14:abc125dcc246 613 {
s1725696 14:abc125dcc246 614 if (counts2 < ((tower_end_position - tower_1_position)/2 + 100))
s1725696 14:abc125dcc246 615 {
s1725696 14:abc125dcc246 616 translation_stop();
s1725696 14:abc125dcc246 617 }
s1725696 14:abc125dcc246 618 }
s1725696 4:8183e7b228f0 619 }
s1725696 0:cb8857cf3ea4 620
s1725696 10:56136a0da8c1 621 // OPERATING
s1725696 0:cb8857cf3ea4 622 // To control the robot with EMG signals
s1725696 11:79311abb2bc2 623 // Kenneth bezig met motor aansturen
s1725696 16:37b491eac34b 624 void boundaries()
s1725696 16:37b491eac34b 625 {
s1725696 16:37b491eac34b 626 double q2tot = q2 + dq2;
s1725696 16:37b491eac34b 627 if (q2tot > q2end)
s1725696 16:37b491eac34b 628 {
s1725696 16:37b491eac34b 629 dq2 = 0;
s1725696 16:37b491eac34b 630 } //kan ook zeggen q2end-q2 is dat dan juiste waarde of moet q2-q2end?
s1725696 16:37b491eac34b 631 else if (q2tot < q2start)
s1725696 16:37b491eac34b 632 {
s1725696 16:37b491eac34b 633 dq2 = 0;
s1725696 16:37b491eac34b 634 }
s1725696 16:37b491eac34b 635 else{}
s1725696 16:37b491eac34b 636 }
s1725696 16:37b491eac34b 637
s1725696 16:37b491eac34b 638 void motor_control()
s1725696 12:b2b082e73ef1 639 {
s1725696 16:37b491eac34b 640 // servocontrol(); // make sure the servo is used in this mode, maybe attach to a ticker?
s1725696 16:37b491eac34b 641 out1 = (pot1*2.0f)-1.0f; //control x-direction
s1725696 16:37b491eac34b 642 out2 = (pot2*2.0f)-1.0f; //control y-direction
s1725696 16:37b491eac34b 643 counts1 = encoder1.getPulses(); //counts encoder 1
s1725696 16:37b491eac34b 644 counts2 = encoder2.getPulses(); //counts encoder 2
s1725696 16:37b491eac34b 645 vdesx = out1 * 20.0; //speed x-direction
s1725696 16:37b491eac34b 646 vdesy = out2 * 20.0; //speed y-direction
s1725696 16:37b491eac34b 647
s1725696 16:37b491eac34b 648 q1 = counts1 * alpha + q1start; //counts to rotation (rad)
s1725696 16:37b491eac34b 649 q2 = counts2 * beta + q2start; //counts to translation (mm)
s1725696 16:37b491eac34b 650 MPe = L1 - L2 + q2; //x location end effector, x-axis along the translation
s1725696 16:37b491eac34b 651 xe = cos(q1) * MPe; //x location in frame 0
s1725696 16:37b491eac34b 652 ye = sin(q1) * MPe; //y location in frame 0
s1725696 16:37b491eac34b 653 gamma = 1.0 /((-1.0 * ye * sin(q1)) - (xe * cos(q1))); //(1 / det(J'')inverse)
s1725696 16:37b491eac34b 654 dq1 = gamma * delta_t * (sin(q1) * vdesx - cos(q1) * vdesy); //target rotation
s1725696 16:37b491eac34b 655 dq2 = gamma * delta_t * (-1.0 * xe * vdesx - ye * vdesy); //target translation
s1725696 16:37b491eac34b 656 boundaries();
s1725696 16:37b491eac34b 657 dC1 = PID_control( dq1, Rot_Kp, Rot_Ki, Rot_Kd, Rot_error, Rot_prev_error) / alpha; //target rotation to counts
s1725696 16:37b491eac34b 658 dC2 = PID_control( dq2, Trans_Kp, Trans_Ki, Trans_Kd, Trans_error, Trans_prev_error) / beta; //target translation to counts
s1725696 16:37b491eac34b 659 pwm1 = 3.0 * (dC1 / delta_t) / 8400.0; //
s1725696 16:37b491eac34b 660 pwm2 = 3.0 * (dC2 / delta_t) / 8400.0; //
s1725696 16:37b491eac34b 661 dirpin.write(pwm1 < 0);
s1725696 16:37b491eac34b 662 pwmpin = fabs (pwm1);
s1725696 16:37b491eac34b 663 dirpin2.write(pwm2 < 0);
s1725696 16:37b491eac34b 664 pwmpin2 = fabs (pwm2);
s1725696 16:37b491eac34b 665 }
s1725696 0:cb8857cf3ea4 666
s1725696 10:56136a0da8c1 667 // DEMO
s1725696 0:cb8857cf3ea4 668 // To control the robot with a written code and write 'HELLO'
s1725696 4:8183e7b228f0 669 // Voor het laatst bewaren
s1725696 10:56136a0da8c1 670 void demo_mode()
s1725696 10:56136a0da8c1 671 {
s1725696 10:56136a0da8c1 672 // code here
s1725696 10:56136a0da8c1 673 }
s1725696 0:cb8857cf3ea4 674
s1725696 10:56136a0da8c1 675 // FAILURE
s1725696 0:cb8857cf3ea4 676 // To shut down the robot after an error etc
s1725696 13:c087c0f64769 677 void failure_mode()
s1725696 10:56136a0da8c1 678 {
s1725696 14:abc125dcc246 679 led_red == 0; // turning red led on to show emergency mode
s1725696 14:abc125dcc246 680
s1725696 14:abc125dcc246 681 // all pwmpins zero
s1725696 14:abc125dcc246 682 pwmpin_1 = 0.0;
s1725696 14:abc125dcc246 683 pwmpin_2 = 0.0;
s1725696 14:abc125dcc246 684
s1725696 14:abc125dcc246 685 // Servo up?
s1725696 14:abc125dcc246 686 // myservo = 0.1;
s1725696 14:abc125dcc246 687
s1725696 14:abc125dcc246 688 pc.printf("Emergency mode, reset system to continue\r\n");
s1725696 10:56136a0da8c1 689 }
s1725696 0:cb8857cf3ea4 690
s1725696 0:cb8857cf3ea4 691 // Main function
s1725696 4:8183e7b228f0 692 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 693 int main()
s1725696 0:cb8857cf3ea4 694 {
s1725696 0:cb8857cf3ea4 695 pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself!
s1725696 0:cb8857cf3ea4 696 pc.printf("Start code\r\n"); // To check if the program starts
s1725696 9:d7a6a3619576 697 pwmpin_1.period_us(60); // Setting period for PWM
s1725696 13:c087c0f64769 698 Filter_tick.attach(&filtering,T); // EMG signals must be filtered all the time! EMG signals filtered every T sec.
s1725696 1:1a8211e1f3f3 699
s1725696 1:1a8211e1f3f3 700 while(true){
s1725696 6:f495a77c2c95 701 // timer
s1725696 6:f495a77c2c95 702 clock_t start; // start the timer
s1725696 6:f495a77c2c95 703 start = clock();
s1725696 9:d7a6a3619576 704 time_overall = (clock() - start) / (double) CLOCKS_PER_SEC;
s1725696 14:abc125dcc246 705 myservo = 0.1; // Keep the pen lifted until servo function is called (operation mode)
s1725696 5:3581013d4505 706
s1725696 6:f495a77c2c95 707 //pc.printf("potmeter value = %f ", potmeter_value);
s1725696 6:f495a77c2c95 708 //pc.printf("counts = %i\r\n", counts);
s1725696 5:3581013d4505 709
s1725696 6:f495a77c2c95 710 // With the help of a switch loop and states we can switch between states and the robot knows what to do
s1725696 9:d7a6a3619576 711 switch(CurrentState)
s1725696 6:f495a77c2c95 712 {
s1725696 6:f495a77c2c95 713 case WAIT:
s1725696 6:f495a77c2c95 714
s1725696 6:f495a77c2c95 715 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 716 {
s1725696 10:56136a0da8c1 717 pc.printf("State is WAIT\r\n");
s1725696 10:56136a0da8c1 718
s1725696 6:f495a77c2c95 719 // Execute WAIT mode
s1725696 10:56136a0da8c1 720 wait_mode();
s1725696 10:56136a0da8c1 721
s1725696 6:f495a77c2c95 722 StateChanged = false; // the state is still WAIT
s1725696 6:f495a77c2c95 723 }
s1725696 6:f495a77c2c95 724
s1725696 12:b2b082e73ef1 725 if(button_motorcal == false) // condition for WAIT --> MOTOR_CAl; button_motorcal press
s1725696 6:f495a77c2c95 726 {
s1725696 6:f495a77c2c95 727 CurrentState = MOTOR_CAL;
s1725696 9:d7a6a3619576 728 pc.printf("State is MOTOR_CAL\r\n");
s1725696 6:f495a77c2c95 729 StateChanged = true;
s1725696 6:f495a77c2c95 730 }
s1725696 7:ec5add330cb3 731
s1725696 12:b2b082e73ef1 732 if (button_emergency == false) // condition for WAIT --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 733 {
s1725696 7:ec5add330cb3 734 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 735 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 736 StateChanged = true;
s1725696 7:ec5add330cb3 737 }
s1725696 6:f495a77c2c95 738
s1725696 6:f495a77c2c95 739 break;
s1725696 6:f495a77c2c95 740
s1725696 6:f495a77c2c95 741 case MOTOR_CAL:
s1725696 6:f495a77c2c95 742
s1725696 6:f495a77c2c95 743 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 744 {
s1725696 9:d7a6a3619576 745 t.reset();
s1725696 6:f495a77c2c95 746 t.start();
s1725696 10:56136a0da8c1 747
s1725696 6:f495a77c2c95 748 // Execute MOTOR_CAL mode
s1725696 10:56136a0da8c1 749 motor_calibration();
s1725696 10:56136a0da8c1 750
s1725696 6:f495a77c2c95 751 t.stop();
s1725696 6:f495a77c2c95 752 time_in_state = t.read();
s1725696 9:d7a6a3619576 753 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 754
s1725696 6:f495a77c2c95 755 StateChanged = false; // the state is still MOTOR_CAL
s1725696 6:f495a77c2c95 756 }
s1725696 6:f495a77c2c95 757
s1725696 15:2772f8cbf382 758 if((time_in_state > 10.0) && (pwmpin_1 < 0.01) && (pwmpin_2 < 0.01)) // condition for MOTOR_CAL --> EMG_CAL; 3s and motors stopped moving
s1725696 6:f495a77c2c95 759 {
s1725696 6:f495a77c2c95 760 CurrentState = EMG_CAL;
s1725696 9:d7a6a3619576 761 pc.printf("State is EMG_CAL\r\n");
s1725696 6:f495a77c2c95 762 StateChanged = true;
s1725696 6:f495a77c2c95 763 }
s1725696 6:f495a77c2c95 764
s1725696 12:b2b082e73ef1 765 if (button_emergency == false) // condition for MOTOR_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 766 {
s1725696 7:ec5add330cb3 767 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 768 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 769 StateChanged = true;
s1725696 7:ec5add330cb3 770 }
s1725696 7:ec5add330cb3 771
s1725696 6:f495a77c2c95 772 break;
s1725696 6:f495a77c2c95 773
s1725696 6:f495a77c2c95 774 case EMG_CAL:
s1725696 6:f495a77c2c95 775
s1725696 6:f495a77c2c95 776 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 777 {
s1725696 6:f495a77c2c95 778 t.reset();
s1725696 6:f495a77c2c95 779 t.start();
s1725696 10:56136a0da8c1 780
s1725696 6:f495a77c2c95 781 // Execute EMG_CAL mode
s1725696 10:56136a0da8c1 782 emg_calibration();
s1725696 10:56136a0da8c1 783
s1725696 6:f495a77c2c95 784 t.stop();
s1725696 6:f495a77c2c95 785 time_in_state = t.read();
s1725696 9:d7a6a3619576 786 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 787
s1725696 6:f495a77c2c95 788 StateChanged = false; // state is still EMG_CAL
s1725696 6:f495a77c2c95 789 }
s1725696 6:f495a77c2c95 790
s1725696 15:2772f8cbf382 791 if((time_in_state > 10.0) && (Average0 < 0.04) && (Average1 < 0.04) && (Average2 < 0.04)) // condition for EMG_CAL --> START; 5s and EMG is low
s1725696 6:f495a77c2c95 792 {
s1725696 6:f495a77c2c95 793 CurrentState = START;
s1725696 9:d7a6a3619576 794 pc.printf("State is START\r\n");
s1725696 6:f495a77c2c95 795 StateChanged = true;
s1725696 6:f495a77c2c95 796 }
s1725696 6:f495a77c2c95 797
s1725696 12:b2b082e73ef1 798 if (button_emergency == false) // condition for EMG_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 799 {
s1725696 7:ec5add330cb3 800 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 801 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 802 StateChanged = true;
s1725696 7:ec5add330cb3 803 }
s1725696 7:ec5add330cb3 804
s1725696 6:f495a77c2c95 805 break;
s1725696 6:f495a77c2c95 806
s1725696 6:f495a77c2c95 807 case START:
s1725696 6:f495a77c2c95 808
s1725696 6:f495a77c2c95 809 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 810 {
s1725696 6:f495a77c2c95 811 t.reset();
s1725696 6:f495a77c2c95 812 t.start();
s1725696 10:56136a0da8c1 813
s1725696 6:f495a77c2c95 814 // Execute START mode
s1725696 10:56136a0da8c1 815 start_mode();
s1725696 10:56136a0da8c1 816
s1725696 6:f495a77c2c95 817 t.stop();
s1725696 6:f495a77c2c95 818 time_in_state = t.read();
s1725696 9:d7a6a3619576 819 pc.printf("Time here = %f\r\n", time_in_state);
s1725696 10:56136a0da8c1 820
s1725696 6:f495a77c2c95 821 StateChanged = false; // state is still START
s1725696 6:f495a77c2c95 822 }
s1725696 6:f495a77c2c95 823
s1725696 15:2772f8cbf382 824 if((time_in_state > 5.0) && (pwmpin_1 < 0.01) && (pwmpin_2 < 0.01)) // condition for START --> OPERATING; 5s and motors stopped moving
s1725696 6:f495a77c2c95 825 {
s1725696 6:f495a77c2c95 826 CurrentState = OPERATING;
s1725696 9:d7a6a3619576 827 pc.printf("State is OPERATING\r\n");
s1725696 6:f495a77c2c95 828 StateChanged = true;
s1725696 6:f495a77c2c95 829 }
s1725696 6:f495a77c2c95 830
s1725696 12:b2b082e73ef1 831 if (button_emergency == false) // condition for START --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 832 {
s1725696 7:ec5add330cb3 833 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 834 pc.printf("State is FAILURE\r\n");
s1725696 7:ec5add330cb3 835 StateChanged = true;
s1725696 7:ec5add330cb3 836 }
s1725696 7:ec5add330cb3 837
s1725696 6:f495a77c2c95 838 break;
s1725696 6:f495a77c2c95 839
s1725696 6:f495a77c2c95 840 case OPERATING:
s1725696 6:f495a77c2c95 841
s1725696 6:f495a77c2c95 842 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 843 {
s1725696 6:f495a77c2c95 844 // Execute OPERATING mode
s1725696 16:37b491eac34b 845 while(1)
s1725696 16:37b491eac34b 846 {
s1725696 16:37b491eac34b 847 motor_control();
s1725696 16:37b491eac34b 848 pc.printf("PWM_rot = %f PWM_trans = %f VdesX = %f VdesY = %f \n\r",pwm1,pwm2,vdesx,vdesy);
s1725696 16:37b491eac34b 849 wait(delta_t);
s1725696 16:37b491eac34b 850 }
s1725696 10:56136a0da8c1 851
s1725696 6:f495a77c2c95 852 StateChanged = false; // state is still OPERATING
s1725696 6:f495a77c2c95 853 }
s1725696 6:f495a77c2c95 854
s1725696 12:b2b082e73ef1 855 if(button_emergency == false) // condition for OPERATING --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 856 {
s1725696 6:f495a77c2c95 857 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 858 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 859 StateChanged = true;
s1725696 6:f495a77c2c95 860 }
s1725696 6:f495a77c2c95 861
s1725696 12:b2b082e73ef1 862 if(button_demo == false) // condition for OPERATING --> DEMO; button_demo press
s1725696 6:f495a77c2c95 863 {
s1725696 6:f495a77c2c95 864 CurrentState = DEMO;
s1725696 9:d7a6a3619576 865 pc.printf("State is DEMO\r\n");
s1725696 9:d7a6a3619576 866 StateChanged = true;
s1725696 9:d7a6a3619576 867 }
s1725696 9:d7a6a3619576 868
s1725696 12:b2b082e73ef1 869 if(button_wait == false) // condition OPERATING --> WAIT; button_wait press
s1725696 9:d7a6a3619576 870 {
s1725696 9:d7a6a3619576 871 CurrentState = WAIT;
s1725696 9:d7a6a3619576 872 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 873 StateChanged = true;
s1725696 6:f495a77c2c95 874 }
s1725696 6:f495a77c2c95 875
s1725696 6:f495a77c2c95 876 break;
s1725696 6:f495a77c2c95 877
s1725696 6:f495a77c2c95 878 case FAILURE:
s1725696 6:f495a77c2c95 879
s1725696 6:f495a77c2c95 880 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 881 {
s1725696 6:f495a77c2c95 882 // Execute FAILURE mode
s1725696 10:56136a0da8c1 883 failure_mode();
s1725696 14:abc125dcc246 884
s1725696 6:f495a77c2c95 885 StateChanged = false; // state is still FAILURE
s1725696 6:f495a77c2c95 886 }
s1725696 14:abc125dcc246 887 // no other states possible, a full reset (push on reset button) is necessary
s1725696 6:f495a77c2c95 888 break;
s1725696 6:f495a77c2c95 889
s1725696 6:f495a77c2c95 890 case DEMO:
s1725696 6:f495a77c2c95 891
s1725696 6:f495a77c2c95 892 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 893 {
s1725696 6:f495a77c2c95 894 // Execute DEMO mode
s1725696 10:56136a0da8c1 895 demo_mode();
s1725696 10:56136a0da8c1 896
s1725696 6:f495a77c2c95 897 StateChanged = false; // state is still DEMO
s1725696 6:f495a77c2c95 898 }
s1725696 6:f495a77c2c95 899
s1725696 12:b2b082e73ef1 900 if(button_wait == false) // condition for DEMO --> WAIT; button_wait press
s1725696 6:f495a77c2c95 901 {
s1725696 6:f495a77c2c95 902 CurrentState = WAIT;
s1725696 9:d7a6a3619576 903 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 904 StateChanged = true;
s1725696 6:f495a77c2c95 905 }
s1725696 6:f495a77c2c95 906
s1725696 12:b2b082e73ef1 907 if (button_emergency == false) // condition for DEMO --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 908 {
s1725696 6:f495a77c2c95 909 CurrentState = FAILURE;
s1725696 9:d7a6a3619576 910 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 911 StateChanged = true;
s1725696 6:f495a77c2c95 912 }
s1725696 6:f495a77c2c95 913
s1725696 6:f495a77c2c95 914 break;
s1725696 6:f495a77c2c95 915
s1725696 6:f495a77c2c95 916 // no default
s1725696 4:8183e7b228f0 917 }
s1725696 6:f495a77c2c95 918
s1725696 6:f495a77c2c95 919 // while loop does not have to loop every time
s1725696 5:3581013d4505 920 }
s1725696 5:3581013d4505 921
s1725696 0:cb8857cf3ea4 922 }