Group 9 BioRobotics / Mbed 2 deprecated the_code

The code without functions and variables filled in

Dependencies:   HIDScope Servo mbed QEI biquadFilter

THE.cpp@27:399ca1f28bd8, 2018-11-02 (annotated)

Committer:
s1725696
Date:
Fri Nov 02 10:00:44 2018 +0000
Revision:
27:399ca1f28bd8
Parent:
26:0b029c8623fe
Child:
28:1481896462ee
with demo state in again

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