Group 9 BioRobotics / Mbed 2 deprecated the_code

The code without functions and variables filled in

Dependencies:   HIDScope Servo mbed QEI biquadFilter

THE.cpp@35:27418c5fefb9, 2018-11-06 (annotated)

Committer:
s1574396
Date:
Tue Nov 06 15:47:28 2018 +0000
Revision:
35:27418c5fefb9
Parent:
34:23eedc10a4f7 
Laatste toevoegingen

Who changed what in which revision?

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