Group 9 BioRobotics / Mbed 2 deprecated the_code

The code without functions and variables filled in

Dependencies:   HIDScope Servo mbed QEI biquadFilter

THE.cpp@17:65943f6e11dc, 2018-11-01 (annotated)

Committer:
s1725696
Date:
Thu Nov 01 12:00:18 2018 +0000
Revision:
17:65943f6e11dc
Parent:
16:37b491eac34b
Child:
18:db53ac017f50
No more bugs, not checked if it works

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