groep 16 / Mbed 2 deprecated Project_BioRobotics_12

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@20:7f1997276ce2, 2018-10-31 (annotated)

Committer:
Mirjam
Date:
Wed Oct 31 16:01:14 2018 +0000
Revision:
20:7f1997276ce2
Parent:
19:2d9421b0316a 
Changed moving speeds of setpoint, set EMG_calibrated_max to zero and extra pc print output during the states

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Mirjam 0:46dbc9b620d8 1 #include "mbed.h"
arnouddomhof 3:dca57056e5cb 2 #include "MODSERIAL.h"
AppelSab 6:a02ad75f0333 3 #include "QEI.h"
AppelSab 6:a02ad75f0333 4 #include "FastPWM.h"
AppelSab 6:a02ad75f0333 5 #include "math.h"
arnouddomhof 8:2afb66572fc4 6 //#include "HIDScope.h"
Mirjam 7:d4090f334ce2 7 #include "BiQuad.h"
Mirjam 7:d4090f334ce2 8 #include "BiQuad4.h"
Mirjam 7:d4090f334ce2 9 #include "FilterDesign.h"
Mirjam 7:d4090f334ce2 10 #include "FilterDesign2.h"
Mirjam 7:d4090f334ce2 11
Mirjam 17:e5d9a543157b 12 const double pi = 3.14159265359;
AppelSab 6:a02ad75f0333 13 // LED's
arnouddomhof 3:dca57056e5cb 14 DigitalOut led_red(LED_RED);
arnouddomhof 3:dca57056e5cb 15 DigitalOut led_green(LED_GREEN);
arnouddomhof 3:dca57056e5cb 16 DigitalOut led_blue(LED_BLUE);
Mirjam 19:2d9421b0316a 17
AppelSab 6:a02ad75f0333 18 // Buttons
arnouddomhof 9:8b2d6ec577e3 19 DigitalIn button_clbrt_home(SW2);
arnouddomhof 9:8b2d6ec577e3 20 DigitalIn button_Demo(D5);
arnouddomhof 9:8b2d6ec577e3 21 DigitalIn button_Emg(D6);
AppelSab 6:a02ad75f0333 22 DigitalIn Fail_button(SW3);
Mirjam 19:2d9421b0316a 23
AppelSab 6:a02ad75f0333 24 // Modserial
arnouddomhof 3:dca57056e5cb 25 MODSERIAL pc(USBTX, USBRX);
Mirjam 19:2d9421b0316a 26
AppelSab 6:a02ad75f0333 27 // Encoders
AppelSab 6:a02ad75f0333 28 QEI Encoder1(D11, D10, NC, 4200) ; // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
AppelSab 6:a02ad75f0333 29 QEI Encoder2(D9, D8, NC, 4200) ; // Encoder motor 2, (pin 2A, pin 2B, index pin (not used), counts/rev)
Mirjam 19:2d9421b0316a 30
AppelSab 6:a02ad75f0333 31 // Motors (direction and PWM)
AppelSab 6:a02ad75f0333 32 DigitalOut directionM1(D4);
AppelSab 6:a02ad75f0333 33 DigitalOut directionM2(D7);
AppelSab 6:a02ad75f0333 34 FastPWM motor1_pwm(D5);
AppelSab 6:a02ad75f0333 35 FastPWM motor2_pwm(D6);
Mirjam 19:2d9421b0316a 36
Mirjam 17:e5d9a543157b 37 // Inverse Kinematics
Mirjam 19:2d9421b0316a 38 int track;
Mirjam 19:2d9421b0316a 39 volatile double U1;
Mirjam 19:2d9421b0316a 40 volatile double U2;
Mirjam 19:2d9421b0316a 41 DigitalIn directionx(PTC2); //x direction switch button
Mirjam 19:2d9421b0316a 42 DigitalIn directiony(PTA2); //y direction switch button
Mirjam 19:2d9421b0316a 43 const double r_big = 590.0; //maximum radius of the moving space
Mirjam 19:2d9421b0316a 44 const double r_small = 162.0; //minimum radius of the moving space
Mirjam 19:2d9421b0316a 45 const double r_top = 250.0; //radius of the top portion of the moving space
Mirjam 20:7f1997276ce2 46 double v=0.02; //moving speed of setpoint (dependant on the waiting time)
Mirjam 19:2d9421b0316a 47 volatile int sx;//value of the button and store as switch
Mirjam 19:2d9421b0316a 48 volatile int sy;//value of the button and store as switch
Mirjam 19:2d9421b0316a 49 int dirx = 1; //determine the direction of the setpoint placement
Mirjam 19:2d9421b0316a 50 int diry = 1; //determine the direction of the setpoint placement
Mirjam 19:2d9421b0316a 51 double q1_diff;
Mirjam 19:2d9421b0316a 52 double q2_diff;
Mirjam 17:e5d9a543157b 53 double sq = 2.0; //to square numbers
Mirjam 19:2d9421b0316a 54 const double x0 = 80.0; //zero x position after homing
Mirjam 19:2d9421b0316a 55 const double y0 = 141.0; //zero y position after homing
Mirjam 17:e5d9a543157b 56 const double L1 = 250.0; //length of the first link
Mirjam 17:e5d9a543157b 57 const double L3 = 350.0; //length of the second link
Mirjam 19:2d9421b0316a 58 volatile double setpointx = x0; //sets the begin condition for x to x0
Mirjam 19:2d9421b0316a 59 volatile double setpointy = y0; //sets the begin condition for y to y0
Mirjam 19:2d9421b0316a 60
Mirjam 17:e5d9a543157b 61 // Reference angles of the starting position
Mirjam 17:e5d9a543157b 62 double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
Mirjam 17:e5d9a543157b 63 double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
Mirjam 17:e5d9a543157b 64 double q2_0_enc = q2_0 + q1_0;
Mirjam 19:2d9421b0316a 65
Mirjam 7:d4090f334ce2 66 // EMG input en start value of filtered EMG
Mirjam 7:d4090f334ce2 67 AnalogIn emg1_raw( A0 );
Mirjam 7:d4090f334ce2 68 AnalogIn emg2_raw( A1 );
Mirjam 7:d4090f334ce2 69 double emg1_filtered = 0.00;
Mirjam 7:d4090f334ce2 70 double emg2_filtered = 0.00;
Mirjam 7:d4090f334ce2 71 float threshold_EMG = 0.25; // Threshold on 25 percent of the maximum EMG
Mirjam 7:d4090f334ce2 72
AppelSab 6:a02ad75f0333 73 // Declare timers and Tickers
Mirjam 7:d4090f334ce2 74 Timer timer; // Timer for counting time in this state
Mirjam 7:d4090f334ce2 75 Ticker WriteValues; // Ticker to show values of velocity to screen
Mirjam 7:d4090f334ce2 76 Ticker StateMachine;
Mirjam 19:2d9421b0316a 77 Ticker sample_EMGtoHIDscope; // Ticker to send the EMG signals to screen
arnouddomhof 8:2afb66572fc4 78 //HIDScope scope(4); //Number of channels which needs to be send to the HIDScope
arnouddomhof 3:dca57056e5cb 79
AppelSab 6:a02ad75f0333 80 // Set up ProcessStateMachine
arnouddomhof 5:07e401cb251d 81 enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_DEMO, FAILURE_MODE};
arnouddomhof 3:dca57056e5cb 82 states currentState = WAITING;
AppelSab 6:a02ad75f0333 83 bool stateChanged = true;
AppelSab 6:a02ad75f0333 84 volatile bool writeVelocityFlag = false;
AppelSab 6:a02ad75f0333 85
AppelSab 6:a02ad75f0333 86 // Global variables
arnouddomhof 3:dca57056e5cb 87 char c;
AppelSab 6:a02ad75f0333 88 int counts1;
AppelSab 6:a02ad75f0333 89 int counts2;
AppelSab 6:a02ad75f0333 90 float theta1;
AppelSab 6:a02ad75f0333 91 float theta2;
AppelSab 6:a02ad75f0333 92 float vel_1;
AppelSab 6:a02ad75f0333 93 float vel_2;
AppelSab 6:a02ad75f0333 94 float theta1_prev = 0.0;
AppelSab 6:a02ad75f0333 95 float theta2_prev = 0.0;
AppelSab 6:a02ad75f0333 96 float tijd = 0.005;
AppelSab 6:a02ad75f0333 97 float time_in_state;
Mirjam 19:2d9421b0316a 98 volatile double error1;
Mirjam 19:2d9421b0316a 99 volatile double error2;
AppelSab 6:a02ad75f0333 100
Mirjam 7:d4090f334ce2 101 int need_to_move_1; // Does the robot needs to move in the first direction?
Mirjam 7:d4090f334ce2 102 int need_to_move_2; // Does the robot needs to move in the second direction?
Mirjam 20:7f1997276ce2 103 double EMG_calibrated_max_1 = 0.00000; // Maximum value of the first EMG signal found in the calibration state.
Mirjam 20:7f1997276ce2 104 double EMG_calibrated_max_2 = 0.00000; // Maximum value of the second EMG signal found in the calibration state.
Duif 10:3f93fdb90c29 105 double emg1_cal = 0.00000; //measured value of the first emg
Duif 10:3f93fdb90c29 106 double emg2_cal = 0.00000; //measured value of the second emg
Mirjam 7:d4090f334ce2 107
AppelSab 6:a02ad75f0333 108 // ----------------------------------------------
AppelSab 6:a02ad75f0333 109 // ------- FUNCTIONS ----------------------------
AppelSab 6:a02ad75f0333 110 // ----------------------------------------------
AppelSab 6:a02ad75f0333 111
AppelSab 6:a02ad75f0333 112 float ReadEncoder1() // Read Encoder of motor 1.
AppelSab 6:a02ad75f0333 113 {
AppelSab 6:a02ad75f0333 114 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
AppelSab 6:a02ad75f0333 115 theta1 = (float(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
AppelSab 6:a02ad75f0333 116 vel_1 = (theta1 - theta1_prev) / tijd; // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
AppelSab 6:a02ad75f0333 117 theta1_prev = theta1; // Define theta_prev
AppelSab 6:a02ad75f0333 118 return vel_1;
AppelSab 6:a02ad75f0333 119 }
AppelSab 6:a02ad75f0333 120 float ReadEncoder2() // Read encoder of motor 2.
AppelSab 6:a02ad75f0333 121 {
AppelSab 6:a02ad75f0333 122 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
AppelSab 6:a02ad75f0333 123 theta2 = (float(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
AppelSab 6:a02ad75f0333 124 vel_2 = (theta2 - theta2_prev) / tijd; // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
AppelSab 6:a02ad75f0333 125 theta2_prev = theta2; // Define theta_prev
AppelSab 6:a02ad75f0333 126 return vel_2;
AppelSab 6:a02ad75f0333 127 }
AppelSab 6:a02ad75f0333 128 void MotorAngleCalibrate() // Function that drives motor 1 and 2.
AppelSab 6:a02ad75f0333 129 {
AppelSab 6:a02ad75f0333 130 float U1 = -0.2; // Negative, so arm goes backwards.
AppelSab 6:a02ad75f0333 131 float U2 = -0.2; // Motor 2 is not taken into account yet.
AppelSab 6:a02ad75f0333 132
AppelSab 6:a02ad75f0333 133 motor1_pwm.write(fabs(U1)); // Send PWM values to motor
AppelSab 6:a02ad75f0333 134 motor2_pwm.write(fabs(U2));
AppelSab 6:a02ad75f0333 135
AppelSab 6:a02ad75f0333 136 directionM1 = U1 > 0.0f; // Either true or false, determines direction.
AppelSab 6:a02ad75f0333 137 directionM2 = U2 > 0.0f;
AppelSab 6:a02ad75f0333 138 }
Mirjam 7:d4090f334ce2 139 void sample()
Mirjam 7:d4090f334ce2 140 {
Mirjam 7:d4090f334ce2 141 emg1_filtered = FilterDesign(emg1_raw.read());
Mirjam 7:d4090f334ce2 142 emg2_filtered = FilterDesign2(emg2_raw.read());
Mirjam 7:d4090f334ce2 143
arnouddomhof 8:2afb66572fc4 144 /**
Mirjam 7:d4090f334ce2 145 scope.set(0, emg1_raw.read()); // Raw EMG 1 send to scope 0
Mirjam 7:d4090f334ce2 146 scope.set(1, emg1_filtered); // Filtered EMG 1 send to scope 1
Mirjam 7:d4090f334ce2 147 scope.set(2, emg2_raw.read()); // Raw EMG 2 send to scope 2
Mirjam 7:d4090f334ce2 148 scope.set(3, emg2_filtered); // Filtered EMG 2 send to scope 3
Mirjam 7:d4090f334ce2 149 scope.send(); // Send the data to the computer
arnouddomhof 8:2afb66572fc4 150 */
Mirjam 7:d4090f334ce2 151 }
Mirjam 17:e5d9a543157b 152
Mirjam 17:e5d9a543157b 153 // ---------------------------------------------------
Mirjam 17:e5d9a543157b 154 // --------INVERSE-KINEMATICS-------------------------
Mirjam 17:e5d9a543157b 155 // ---------------------------------------------------
Mirjam 17:e5d9a543157b 156 double makeAngleq1(double x, double y){
Mirjam 17:e5d9a543157b 157 double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
Mirjam 17:e5d9a543157b 158 q1_diff = -2.0*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint
Mirjam 17:e5d9a543157b 159 return q1_diff;
Mirjam 17:e5d9a543157b 160 }
Mirjam 17:e5d9a543157b 161
Mirjam 17:e5d9a543157b 162 double makeAngleq2(double x, double y){
Mirjam 17:e5d9a543157b 163 double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
Mirjam 17:e5d9a543157b 164 double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
Mirjam 17:e5d9a543157b 165 double q2_motor = (pi - q2)+q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
Mirjam 17:e5d9a543157b 166 q2_diff = (2.0*(q2_motor - q2_0_enc))/(2.0*pi); //the actual amount of radians that the motor has to turn in total to reach the setpoint
Mirjam 17:e5d9a543157b 167 return -q2_diff;
Mirjam 17:e5d9a543157b 168 }
Mirjam 17:e5d9a543157b 169
Mirjam 19:2d9421b0316a 170
Mirjam 19:2d9421b0316a 171 // ---------------------------------------------------
Mirjam 19:2d9421b0316a 172 // --------DETERMINE SETPOINT-------------------------
Mirjam 19:2d9421b0316a 173 // ---------------------------------------------------
Mirjam 19:2d9421b0316a 174 //function that determines the setpoint of the x coordinate
Mirjam 19:2d9421b0316a 175 double EMG1On(int s){
Mirjam 19:2d9421b0316a 176 if (setpointx < 80.0){ //minimum setpoint
Mirjam 19:2d9421b0316a 177 setpointx = setpointx;}
Mirjam 19:2d9421b0316a 178 if (setpointy > -66.0 && setpointy < 362.0 && sqrt(pow(setpointx,sq)+pow(setpointy,sq)) > r_big){ //defines the large circle endpoint
Mirjam 19:2d9421b0316a 179 setpointx = setpointx;}
Mirjam 19:2d9421b0316a 180 if (setpointy > 141.0 && setpointx < 540.0 && sqrt(pow(setpointy-119.2,sq)+pow(setpointx-329.0,sq)) > r_top){ //defines the top circle endpoint
Mirjam 19:2d9421b0316a 181 setpointx = setpointx;}
Mirjam 19:2d9421b0316a 182 if (setpointx > 80.0 && setpointy > -36.0 && sqrt(pow(setpointx,sq)+pow(setpointy,sq)) < r_small){ //defines the small circle endpoint
Mirjam 19:2d9421b0316a 183 setpointx = setpointx;}
Mirjam 19:2d9421b0316a 184 if (setpointy > -66.0 && setpointy < -36.0 && setpointx < 157.0){ //kleine stukje
Mirjam 19:2d9421b0316a 185 setpointx = setpointx+2;}
Mirjam 19:2d9421b0316a 186 else setpointx = setpointx + dirx*s*v;
Mirjam 19:2d9421b0316a 187 return setpointx;
Mirjam 19:2d9421b0316a 188 }
Mirjam 19:2d9421b0316a 189
Mirjam 19:2d9421b0316a 190 //function that determines the setpoint of the y coordinate
Mirjam 19:2d9421b0316a 191 double EMG2On(int s){
Mirjam 19:2d9421b0316a 192 if (setpointy < -66.0) //bottom line
Mirjam 19:2d9421b0316a 193 setpointy = setpointy;
Mirjam 19:2d9421b0316a 194 if (setpointy > -66.0 && setpointy < 362.0 && sqrt(pow(setpointx,sq)+pow(setpointy,sq)) > r_big){ //defines the large circle endpoint
Mirjam 19:2d9421b0316a 195 setpointy = setpointy;}
Mirjam 19:2d9421b0316a 196 if (setpointy >= 141.0 && setpointx < 540.0 && sqrt(pow(setpointy-119.2,sq)+pow(setpointx-329.0,sq)) > r_top){ //defines the top circle endpoint
Mirjam 19:2d9421b0316a 197 setpointy = setpointy;}
Mirjam 19:2d9421b0316a 198 if (setpointx > 80.0 && setpointy > -36.0 && sqrt(pow(setpointx,sq)+pow(setpointy,sq)) < r_small){ //defines the small circle endpoint
Mirjam 19:2d9421b0316a 199 setpointy = setpointy;}
Mirjam 19:2d9421b0316a 200 else setpointy = setpointy + diry*s*v;
Mirjam 19:2d9421b0316a 201 return setpointy;
Mirjam 19:2d9421b0316a 202 }
Mirjam 19:2d9421b0316a 203
Mirjam 19:2d9421b0316a 204 //function to change the moving direction of the setpoint
Mirjam 19:2d9421b0316a 205 void ChangeDirectionX(){
Mirjam 19:2d9421b0316a 206 dirx = -1*dirx;
Mirjam 19:2d9421b0316a 207 }
Mirjam 19:2d9421b0316a 208
Mirjam 19:2d9421b0316a 209 void ChangeDirectionY(){
Mirjam 19:2d9421b0316a 210 diry = -1*diry;
Mirjam 19:2d9421b0316a 211 }
Mirjam 19:2d9421b0316a 212
Mirjam 19:2d9421b0316a 213
Mirjam 17:e5d9a543157b 214 // --------------------------------------------------------------------
Mirjam 17:e5d9a543157b 215 // ---------------READ-OUT ENCODERS------------------------------------
Mirjam 17:e5d9a543157b 216 // --------------------------------------------------------------------
Mirjam 17:e5d9a543157b 217 double counts2angle1()
Mirjam 17:e5d9a543157b 218 {
Mirjam 17:e5d9a543157b 219 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
Mirjam 17:e5d9a543157b 220 theta1 = -(double(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
Mirjam 17:e5d9a543157b 221 return theta1;
Mirjam 17:e5d9a543157b 222 }
Mirjam 17:e5d9a543157b 223
Mirjam 17:e5d9a543157b 224 double counts2angle2()
Mirjam 17:e5d9a543157b 225 {
Mirjam 17:e5d9a543157b 226 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
Mirjam 17:e5d9a543157b 227 theta2 = (double(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
Mirjam 17:e5d9a543157b 228 return theta2;
Mirjam 17:e5d9a543157b 229 }
Mirjam 17:e5d9a543157b 230
Mirjam 17:e5d9a543157b 231 // -----------------------------------------------------------------
Mirjam 17:e5d9a543157b 232 // --------------------------- PI controllers ----------------------
Mirjam 17:e5d9a543157b 233 // -----------------------------------------------------------------
Mirjam 17:e5d9a543157b 234 double PI_controller1(double error1)
Mirjam 17:e5d9a543157b 235 {
Mirjam 17:e5d9a543157b 236 static double error_integral1 = 0;
Mirjam 17:e5d9a543157b 237
Mirjam 17:e5d9a543157b 238 // Proportional part
Mirjam 17:e5d9a543157b 239 double Kp1 = 3.95; // Kp (proportionele controller, nu nog een random waarde)
Mirjam 17:e5d9a543157b 240 double u_p1 = Kp1*error1; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
Mirjam 17:e5d9a543157b 241
Mirjam 17:e5d9a543157b 242 // Integral part
Mirjam 17:e5d9a543157b 243 double Ki1 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde)
Mirjam 17:e5d9a543157b 244 double Ts1 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
Mirjam 17:e5d9a543157b 245 error_integral1 = error_integral1 + error1 * Ts1;
Mirjam 17:e5d9a543157b 246 double u_i1 = Ki1 * error_integral1;
Mirjam 17:e5d9a543157b 247
Mirjam 17:e5d9a543157b 248 // Sum
Mirjam 17:e5d9a543157b 249 U1 = u_p1 + u_i1;
Mirjam 17:e5d9a543157b 250
Mirjam 17:e5d9a543157b 251 // Return
Mirjam 17:e5d9a543157b 252 return U1;
Mirjam 17:e5d9a543157b 253 }
Mirjam 17:e5d9a543157b 254 double PI_controller2(double error2)
Mirjam 17:e5d9a543157b 255 {
Mirjam 17:e5d9a543157b 256 static double error_integral2 = 0;
Mirjam 17:e5d9a543157b 257
Mirjam 17:e5d9a543157b 258 // Proportional part
Mirjam 17:e5d9a543157b 259 double Kp2 = 3.95; // Kp (proportionele controller, nu nog een random waarde)
Mirjam 17:e5d9a543157b 260 double u_p2 = Kp2*error2; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
Mirjam 17:e5d9a543157b 261
Mirjam 17:e5d9a543157b 262 // Integral part
Mirjam 17:e5d9a543157b 263 double Ki2 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde)
Mirjam 17:e5d9a543157b 264 double Ts2 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
Mirjam 17:e5d9a543157b 265 error_integral2 = error_integral2 + error2 * Ts2;
Mirjam 17:e5d9a543157b 266 double u_i2 = Ki2 * error_integral2;
Mirjam 17:e5d9a543157b 267
Mirjam 17:e5d9a543157b 268 // Sum +
Mirjam 17:e5d9a543157b 269 U2 = u_p2 + u_i2;
Mirjam 17:e5d9a543157b 270
Mirjam 17:e5d9a543157b 271 // Return
Mirjam 17:e5d9a543157b 272 return U2;
Mirjam 17:e5d9a543157b 273 }
Mirjam 17:e5d9a543157b 274
Mirjam 17:e5d9a543157b 275 // -----------------------------------------------
Mirjam 17:e5d9a543157b 276 // ------------ RUN MOTORS -----------------------
Mirjam 17:e5d9a543157b 277 // -----------------------------------------------
Mirjam 17:e5d9a543157b 278 void motoraansturing()
Mirjam 19:2d9421b0316a 279 {
Mirjam 19:2d9421b0316a 280
Mirjam 17:e5d9a543157b 281 q1_diff = makeAngleq1(setpointx, setpointy);
Mirjam 17:e5d9a543157b 282 q2_diff = makeAngleq2(setpointx, setpointy);
Mirjam 17:e5d9a543157b 283
Mirjam 17:e5d9a543157b 284 theta2 = counts2angle2();
Mirjam 17:e5d9a543157b 285 error2 = q2_diff - theta2;
Mirjam 17:e5d9a543157b 286 theta1 = counts2angle1();
Mirjam 17:e5d9a543157b 287 error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as.
Mirjam 17:e5d9a543157b 288
Mirjam 17:e5d9a543157b 289 U1 = PI_controller1(error1); // Voltage dat naar de motor gestuurd wordt.
Mirjam 17:e5d9a543157b 290 U2 = PI_controller2(error2);
Mirjam 17:e5d9a543157b 291
Mirjam 17:e5d9a543157b 292 motor1_pwm.write(fabs(U1)); // Motor aansturen
Mirjam 17:e5d9a543157b 293 directionM1 = U1 > 0.0f; // Richting van de motor bepalen
Mirjam 17:e5d9a543157b 294 motor2_pwm.write(fabs(U2));
Mirjam 17:e5d9a543157b 295 directionM2 = U2 > 0.0f;
Mirjam 17:e5d9a543157b 296 }
Mirjam 17:e5d9a543157b 297
AppelSab 6:a02ad75f0333 298 // ---------------------------------------------------
AppelSab 6:a02ad75f0333 299 // --------STATEMACHINE-------------------------------
AppelSab 6:a02ad75f0333 300 // ---------------------------------------------------
AppelSab 6:a02ad75f0333 301 void ProcessStateMachine(void)
AppelSab 6:a02ad75f0333 302 {
AppelSab 6:a02ad75f0333 303 switch (currentState)
AppelSab 6:a02ad75f0333 304 {
AppelSab 6:a02ad75f0333 305 case WAITING:
Mirjam 17:e5d9a543157b 306 // Description:
Mirjam 17:e5d9a543157b 307 // In this state we do nothing, and wait for a command
Mirjam 17:e5d9a543157b 308
Mirjam 17:e5d9a543157b 309 // Actions
Mirjam 17:e5d9a543157b 310 led_red = 0; led_green = 0; led_blue = 0; // Colouring the led WHITE
Mirjam 20:7f1997276ce2 311 pc.printf("I am WAITING");
Mirjam 17:e5d9a543157b 312 // State transition logic
Mirjam 17:e5d9a543157b 313 if (button_clbrt_home == 0)
Mirjam 17:e5d9a543157b 314 {
Mirjam 17:e5d9a543157b 315 currentState = MOTOR_ANGLE_CLBRT;
Mirjam 17:e5d9a543157b 316 stateChanged = true;
Mirjam 17:e5d9a543157b 317 pc.printf("Starting Calibration\n\r");
Mirjam 17:e5d9a543157b 318 }
Mirjam 17:e5d9a543157b 319 else if (Fail_button == 0)
Mirjam 17:e5d9a543157b 320 {
Mirjam 17:e5d9a543157b 321 currentState = FAILURE_MODE;
Mirjam 17:e5d9a543157b 322 stateChanged = true;
Mirjam 17:e5d9a543157b 323 }
Mirjam 17:e5d9a543157b 324 break;
AppelSab 6:a02ad75f0333 325
AppelSab 6:a02ad75f0333 326 case MOTOR_ANGLE_CLBRT:
Mirjam 17:e5d9a543157b 327 // Description:
Mirjam 17:e5d9a543157b 328 // In this state the robot moves with low motor PWM to some
Mirjam 17:e5d9a543157b 329 // mechanical limit of motion, in order to calibrate the motors.
Mirjam 17:e5d9a543157b 330
Mirjam 17:e5d9a543157b 331 // Actions
Mirjam 17:e5d9a543157b 332 led_red = 1; led_green = 0; led_blue = 0; // Colouring the led TURQUOISE
Mirjam 20:7f1997276ce2 333 pc.printf("Motor angle calibrating....");
Mirjam 17:e5d9a543157b 334 timer.start(); //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT"
Mirjam 17:e5d9a543157b 335 if (stateChanged)
Mirjam 17:e5d9a543157b 336 {
Mirjam 17:e5d9a543157b 337 MotorAngleCalibrate(); // Actuate motor 1 and 2.
Mirjam 17:e5d9a543157b 338 vel_1 = ReadEncoder1(); // Get velocity of motor 1
Mirjam 17:e5d9a543157b 339 vel_2 = ReadEncoder2(); // Get velocity of motor 2
Mirjam 17:e5d9a543157b 340 stateChanged = true; // Keep this loop going, until the transition conditions are satisfied.
Mirjam 17:e5d9a543157b 341 }
Mirjam 17:e5d9a543157b 342
Mirjam 17:e5d9a543157b 343 // State transition logic
Mirjam 17:e5d9a543157b 344 time_in_state = timer.read(); // Determine if this state has run for long enough.
Mirjam 17:e5d9a543157b 345
Mirjam 17:e5d9a543157b 346 if(time_in_state > 2.0f && vel_1 < 1.1f && vel_2 < 1.1f)
Mirjam 17:e5d9a543157b 347 {
Mirjam 17:e5d9a543157b 348 //pc.printf( "Tijd in deze staat = %f \n\r", time_in_state);
Mirjam 17:e5d9a543157b 349 //pc.printf( "Tijd tijdens actions loop (Waarde voor bepalen van snelheid)") = %f \n\r", tijd);
Mirjam 17:e5d9a543157b 350 pc.printf("Motor calibration has ended. \n\r");
Mirjam 17:e5d9a543157b 351 timer.stop(); // Stop timer for this state
Mirjam 17:e5d9a543157b 352 timer.reset(); // Reset timer for this state
Mirjam 17:e5d9a543157b 353 motor1_pwm.write(fabs(0.0)); // Send PWM values to motor
Mirjam 17:e5d9a543157b 354 motor2_pwm.write(fabs(0.0));
Mirjam 17:e5d9a543157b 355 Encoder1.reset(); // Reset Encoders when arrived at zero-position
Mirjam 17:e5d9a543157b 356 Encoder2.reset();
Mirjam 17:e5d9a543157b 357
Mirjam 17:e5d9a543157b 358 currentState = EMG_CLBRT; // Switch to next state (EMG_CLRBRT).
Mirjam 20:7f1997276ce2 359 pc.printf("Go to EMG calibration \r\n");
Mirjam 17:e5d9a543157b 360 stateChanged = true;
Mirjam 17:e5d9a543157b 361 }
Mirjam 17:e5d9a543157b 362 if (Fail_button == 0)
AppelSab 6:a02ad75f0333 363 {
Mirjam 17:e5d9a543157b 364 currentState = FAILURE_MODE;
Mirjam 17:e5d9a543157b 365 stateChanged = true;
Mirjam 17:e5d9a543157b 366 }
Mirjam 17:e5d9a543157b 367 break;
AppelSab 6:a02ad75f0333 368
AppelSab 6:a02ad75f0333 369 case EMG_CLBRT:
Mirjam 17:e5d9a543157b 370 // In this state the person whom is connected to the robot needs
Mirjam 17:e5d9a543157b 371 // to flex his/her muscles as hard as possible, in order to
Mirjam 17:e5d9a543157b 372 // measure the maximum EMG-signal, which can be used to scale
Mirjam 17:e5d9a543157b 373 // the EMG-filter.
Mirjam 20:7f1997276ce2 374 led_red = 1; led_green = 1; led_blue = 0; // Colouring the led BLUE
Mirjam 20:7f1997276ce2 375 pc.printf("EMG is calibrating");
Mirjam 18:f36ac3ee081a 376 for (int i = 0; i <= 10; i++) //10 measuring points
Mirjam 18:f36ac3ee081a 377 {
Mirjam 18:f36ac3ee081a 378 if (emg1_cal > EMG_calibrated_max_1){
Mirjam 18:f36ac3ee081a 379 EMG_calibrated_max_1 = emg1_cal;}
AppelSab 6:a02ad75f0333 380
Mirjam 18:f36ac3ee081a 381 if (emg2_cal > EMG_calibrated_max_2){
Mirjam 18:f36ac3ee081a 382 EMG_calibrated_max_2 = emg2_cal;}
Mirjam 18:f36ac3ee081a 383
Mirjam 20:7f1997276ce2 384 pc.printf("EMG1 = %f, EMG2 = %f \r\n",EMG_calibrated_max_1,EMG_calibrated_max_2);
Mirjam 18:f36ac3ee081a 385 wait(0.5f);
Mirjam 18:f36ac3ee081a 386 }
Duif 10:3f93fdb90c29 387
Mirjam 20:7f1997276ce2 388 currentState = HOMING;
Mirjam 20:7f1997276ce2 389 pc.printf("Calibreren klaar, ik ga weer naar HOMING");
Mirjam 17:e5d9a543157b 390 if (Fail_button == 0)
Mirjam 17:e5d9a543157b 391 {
Mirjam 17:e5d9a543157b 392 currentState = FAILURE_MODE;
Mirjam 17:e5d9a543157b 393 stateChanged = true;
Mirjam 17:e5d9a543157b 394 }
Mirjam 17:e5d9a543157b 395 break;
AppelSab 6:a02ad75f0333 396
AppelSab 6:a02ad75f0333 397 case HOMING:
Mirjam 17:e5d9a543157b 398 // Description:
Mirjam 17:e5d9a543157b 399 // Robot moves to the home starting configuration
Mirjam 20:7f1997276ce2 400 pc.printf("HOMING... \r\n");
Mirjam 17:e5d9a543157b 401
Mirjam 17:e5d9a543157b 402 led_red = 0; led_green = 1; led_red = 0; // Colouring the led PURPLE
Mirjam 17:e5d9a543157b 403
Mirjam 17:e5d9a543157b 404 // Requirements to move to the next state:
Mirjam 17:e5d9a543157b 405 // If we are in the right location, within some margin, we move to the Waiting for
Mirjam 17:e5d9a543157b 406 // signal state.
AppelSab 6:a02ad75f0333 407
Mirjam 17:e5d9a543157b 408 wait(5.0f); // time_in_this_state > 5.0f
Mirjam 17:e5d9a543157b 409 // INSERT MOVEMENT
Mirjam 17:e5d9a543157b 410 currentState = WAITING4SIGNAL;
Mirjam 17:e5d9a543157b 411 if (Fail_button == 0)
Mirjam 17:e5d9a543157b 412 {
Mirjam 17:e5d9a543157b 413 currentState = FAILURE_MODE;
Mirjam 17:e5d9a543157b 414 stateChanged = true;
Mirjam 17:e5d9a543157b 415 }
Mirjam 17:e5d9a543157b 416 break;
AppelSab 6:a02ad75f0333 417
AppelSab 6:a02ad75f0333 418 case WAITING4SIGNAL:
Mirjam 17:e5d9a543157b 419 // Description:
Mirjam 17:e5d9a543157b 420 // In this state the robot waits for an action to occur.
Mirjam 17:e5d9a543157b 421
Mirjam 17:e5d9a543157b 422 led_red = 0; led_green = 0; led_blue = 0; // Colouring the led WHITE
Mirjam 20:7f1997276ce2 423 pc.printf("I am waiting for a signal...");
Mirjam 17:e5d9a543157b 424 // Requirements to move to the next state:
Mirjam 17:e5d9a543157b 425 // If a certain button is pressed we move to the corresponding
Mirjam 17:e5d9a543157b 426 // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
Mirjam 17:e5d9a543157b 427
Mirjam 17:e5d9a543157b 428 if (button_clbrt_home == 0)
Mirjam 17:e5d9a543157b 429 {
Mirjam 17:e5d9a543157b 430 currentState = MOTOR_ANGLE_CLBRT;
Mirjam 17:e5d9a543157b 431 stateChanged = true;
Mirjam 17:e5d9a543157b 432 pc.printf("Starting Calibration \n\r");
Mirjam 17:e5d9a543157b 433 }
Mirjam 20:7f1997276ce2 434 else if (button_Demo == 0)
Mirjam 17:e5d9a543157b 435 {
Mirjam 17:e5d9a543157b 436 currentState = MOVE_W_DEMO;
Mirjam 17:e5d9a543157b 437 pc.printf("DEMO \r\n");
Mirjam 17:e5d9a543157b 438 wait(1.0f);
Mirjam 17:e5d9a543157b 439 }
Mirjam 20:7f1997276ce2 440 else if (button_Emg == 0)
Mirjam 17:e5d9a543157b 441 {
Mirjam 17:e5d9a543157b 442 currentState = MOVE_W_EMG;
Mirjam 17:e5d9a543157b 443 pc.printf("EMG \r\n");
Mirjam 17:e5d9a543157b 444 wait(1.0f);
Mirjam 17:e5d9a543157b 445 }
Mirjam 17:e5d9a543157b 446 else if (Fail_button == 0)
Mirjam 17:e5d9a543157b 447 {
Mirjam 17:e5d9a543157b 448 currentState = FAILURE_MODE;
Mirjam 17:e5d9a543157b 449 stateChanged = true;
Mirjam 17:e5d9a543157b 450 }
Mirjam 17:e5d9a543157b 451
Mirjam 17:e5d9a543157b 452 break;
AppelSab 6:a02ad75f0333 453
AppelSab 6:a02ad75f0333 454 case MOVE_W_DEMO:
AppelSab 6:a02ad75f0333 455 // Description:
AppelSab 6:a02ad75f0333 456 // In this state the robot follows a preprogrammed shape, e.g.
AppelSab 6:a02ad75f0333 457 // a square.
AppelSab 6:a02ad75f0333 458
AppelSab 6:a02ad75f0333 459 led_red = 1; led_green = 1; led_blue = 0; // Colouring the led GREEN
Mirjam 20:7f1997276ce2 460 pc.printf("I am moving with a demo");
AppelSab 6:a02ad75f0333 461 // Requirements to move to the next state:
AppelSab 6:a02ad75f0333 462 // When the home button or the failure button is pressed, we
AppelSab 6:a02ad75f0333 463 // will the move to the corresponding state.
AppelSab 6:a02ad75f0333 464
AppelSab 6:a02ad75f0333 465 // BUILD DEMO MODE
Mirjam 19:2d9421b0316a 466 // determinedemoset();
Mirjam 19:2d9421b0316a 467 motoraansturing();
arnouddomhof 9:8b2d6ec577e3 468
arnouddomhof 9:8b2d6ec577e3 469 if (button_clbrt_home == 0)
AppelSab 6:a02ad75f0333 470 {
arnouddomhof 9:8b2d6ec577e3 471 currentState = HOMING;
arnouddomhof 9:8b2d6ec577e3 472 stateChanged = true;
arnouddomhof 9:8b2d6ec577e3 473 pc.printf("Moving home\n\r");
AppelSab 6:a02ad75f0333 474 }
arnouddomhof 9:8b2d6ec577e3 475 else if (Fail_button == 0)
AppelSab 6:a02ad75f0333 476 {
AppelSab 6:a02ad75f0333 477 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 478 stateChanged = true;
AppelSab 6:a02ad75f0333 479 }
AppelSab 6:a02ad75f0333 480 break;
AppelSab 6:a02ad75f0333 481
AppelSab 6:a02ad75f0333 482 case MOVE_W_EMG:
Mirjam 17:e5d9a543157b 483 // Description:
Mirjam 17:e5d9a543157b 484 // In this state the robot will be controlled by use of
Mirjam 17:e5d9a543157b 485 // EMG-signals.
Mirjam 17:e5d9a543157b 486
Mirjam 17:e5d9a543157b 487 led_red = 1; led_green = 0; led_blue = 1; // Colouring the led GREEN
Mirjam 20:7f1997276ce2 488 pc.printf("I am moving with EMG signals...");
Mirjam 17:e5d9a543157b 489 if (emg1_filtered >= (threshold_EMG*EMG_calibrated_max_1)){
Mirjam 17:e5d9a543157b 490 need_to_move_1 = 1; // The robot does have to move
Mirjam 17:e5d9a543157b 491 }
Mirjam 17:e5d9a543157b 492 else {
Mirjam 17:e5d9a543157b 493 need_to_move_1 = 0; // If the robot does not have to move
Mirjam 17:e5d9a543157b 494 }
Mirjam 17:e5d9a543157b 495
Mirjam 17:e5d9a543157b 496 if(emg2_filtered >= threshold_EMG*EMG_calibrated_max_2){
Mirjam 17:e5d9a543157b 497 need_to_move_2 = 1;
Mirjam 17:e5d9a543157b 498 }
Mirjam 17:e5d9a543157b 499 else {
Mirjam 17:e5d9a543157b 500 need_to_move_2 = 0;
Mirjam 17:e5d9a543157b 501 }
Mirjam 17:e5d9a543157b 502
Mirjam 19:2d9421b0316a 503 setpointx = EMG1On(need_to_move_1); // Determine setpoints
Mirjam 19:2d9421b0316a 504 setpointy = EMG2On(need_to_move_2);
Mirjam 20:7f1997276ce2 505 pc.printf("X moet %f worden en Y moet %f worden",setpointx, setpointy);
Mirjam 19:2d9421b0316a 506 motoraansturing();
Mirjam 17:e5d9a543157b 507
Mirjam 17:e5d9a543157b 508 // Requirements to move to the next state:
Mirjam 17:e5d9a543157b 509 // When the home button or the failure button is pressed, we
Mirjam 17:e5d9a543157b 510 // will the move to the corresponding state.
Mirjam 17:e5d9a543157b 511
Mirjam 17:e5d9a543157b 512 if (button_clbrt_home == 0)
Mirjam 17:e5d9a543157b 513 {
Mirjam 17:e5d9a543157b 514 currentState = MOTOR_ANGLE_CLBRT;
Mirjam 17:e5d9a543157b 515 stateChanged = true;
Mirjam 17:e5d9a543157b 516 pc.printf("Starting Calibration \n\r");
Mirjam 17:e5d9a543157b 517 }
Mirjam 17:e5d9a543157b 518 else if (Fail_button == 0)
Mirjam 17:e5d9a543157b 519 {
Mirjam 17:e5d9a543157b 520 currentState = FAILURE_MODE;
Mirjam 17:e5d9a543157b 521 stateChanged = true;
Mirjam 7:d4090f334ce2 522 }
Mirjam 17:e5d9a543157b 523 break;
AppelSab 6:a02ad75f0333 524
AppelSab 6:a02ad75f0333 525 case FAILURE_MODE:
Mirjam 17:e5d9a543157b 526 // Description:
Mirjam 17:e5d9a543157b 527 // This state is reached when the failure button is reached.
Mirjam 17:e5d9a543157b 528 // In this state everything is turned off.
Mirjam 17:e5d9a543157b 529
Mirjam 17:e5d9a543157b 530 led_red = 0; led_green = 1; led_blue = 1; // Colouring the led RED
Mirjam 17:e5d9a543157b 531 // Actions
Mirjam 17:e5d9a543157b 532 if (stateChanged)
Mirjam 17:e5d9a543157b 533 {
Mirjam 17:e5d9a543157b 534 motor1_pwm.write(fabs(0.0)); // Stop all motors!
Mirjam 17:e5d9a543157b 535 motor2_pwm.write(fabs(0.0));
Mirjam 17:e5d9a543157b 536 pc.printf("FAILURE MODE \r\n PLEASE RESTART THE WHOLE ROBOT \r\n (and make sure this does not happen again) \r\n");
Mirjam 17:e5d9a543157b 537 stateChanged = false;
Mirjam 17:e5d9a543157b 538 }
Mirjam 17:e5d9a543157b 539 break;
AppelSab 6:a02ad75f0333 540
AppelSab 6:a02ad75f0333 541 // State transition logic
AppelSab 6:a02ad75f0333 542 // No state transition, you need to restart the robot.
AppelSab 6:a02ad75f0333 543
AppelSab 6:a02ad75f0333 544 default:
AppelSab 6:a02ad75f0333 545 // This state is a default state, this state is reached when
AppelSab 6:a02ad75f0333 546 // the program somehow defies all of the other states.
AppelSab 6:a02ad75f0333 547
AppelSab 6:a02ad75f0333 548 pc.printf("Unknown or unimplemented state reached!!! \n\r");
AppelSab 6:a02ad75f0333 549 led_red = 1; led_green = 1; led_blue = 1; // Colouring the led BLACK
AppelSab 6:a02ad75f0333 550 for (int n = 0; n < 50; n++) // Making an SOS signal with the RED led
AppelSab 6:a02ad75f0333 551 {
AppelSab 6:a02ad75f0333 552 for (int i = 0; i < 6; i++)
AppelSab 6:a02ad75f0333 553 {
AppelSab 6:a02ad75f0333 554 led_red = !led_red;
AppelSab 6:a02ad75f0333 555 wait(0.6f);
AppelSab 6:a02ad75f0333 556 }
AppelSab 6:a02ad75f0333 557 wait(0.4f);
AppelSab 6:a02ad75f0333 558 for (int i = 0 ; i < 6; i++)
AppelSab 6:a02ad75f0333 559 {
AppelSab 6:a02ad75f0333 560 led_red = !led_red;
AppelSab 6:a02ad75f0333 561 wait(0.2f);
AppelSab 6:a02ad75f0333 562 }
AppelSab 6:a02ad75f0333 563 wait(0.4f);
AppelSab 6:a02ad75f0333 564 }
arnouddomhof 3:dca57056e5cb 565 }
AppelSab 6:a02ad75f0333 566 }
AppelSab 6:a02ad75f0333 567
AppelSab 6:a02ad75f0333 568 // --------------------------------
AppelSab 6:a02ad75f0333 569 // ----- MAIN LOOP ----------------
AppelSab 6:a02ad75f0333 570 // --------------------------------
AppelSab 6:a02ad75f0333 571
Mirjam 0:46dbc9b620d8 572 int main()
Mirjam 0:46dbc9b620d8 573 {
Mirjam 4:a0c1c021026b 574 // Switch all LEDs off
arnouddomhof 3:dca57056e5cb 575 led_red = 1;
arnouddomhof 3:dca57056e5cb 576 led_green = 1;
arnouddomhof 3:dca57056e5cb 577 led_blue = 1;
AppelSab 6:a02ad75f0333 578
arnouddomhof 3:dca57056e5cb 579 pc.baud(115200);
arnouddomhof 8:2afb66572fc4 580
arnouddomhof 8:2afb66572fc4 581 pc.printf("\r\n _______________ INSERT ROBOT NAME HERE! _______________ \r\n");
arnouddomhof 8:2afb66572fc4 582 wait(0.5f);
arnouddomhof 8:2afb66572fc4 583 pc.printf("WAITING... \r\n");
arnouddomhof 8:2afb66572fc4 584
AppelSab 6:a02ad75f0333 585 StateMachine.attach(&ProcessStateMachine, 0.005f); // Run statemachine 200 times per second
Mirjam 7:d4090f334ce2 586 sample_EMGtoHIDscope.attach(&sample, 0.02f); // Display EMG values 50 times per second
Mirjam 17:e5d9a543157b 587
Mirjam 0:46dbc9b620d8 588 while (true) {
Mirjam 19:2d9421b0316a 589 if (currentState == MOVE_W_EMG){
Mirjam 19:2d9421b0316a 590 InterruptIn directionx(PTC2);
Mirjam 19:2d9421b0316a 591 directionx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction
Mirjam 19:2d9421b0316a 592 InterruptIn directiony(PTA2);
Mirjam 19:2d9421b0316a 593 directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
Mirjam 19:2d9421b0316a 594 }
AppelSab 6:a02ad75f0333 595 }
AppelSab 6:a02ad75f0333 596 }
AppelSab 6:a02ad75f0333 597
arnouddomhof 5:07e401cb251d 598