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@12:3e084e1a699e, 2018-11-01 (annotated)

Committer:
AppelSab
Date:
Thu Nov 01 17:53:13 2018 +0000
Revision:
12:3e084e1a699e
Parent:
10:3f93fdb90c29
Child:
13:a2e281d5de89
StateMachine met Demomode

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"
Mirjam 7:d4090f334ce2 6 #include "BiQuad.h"
Mirjam 7:d4090f334ce2 7 #include "BiQuad4.h"
Mirjam 7:d4090f334ce2 8 #include "FilterDesign.h"
Mirjam 7:d4090f334ce2 9 #include "FilterDesign2.h"
Mirjam 7:d4090f334ce2 10
AppelSab 6:a02ad75f0333 11 // LED's
arnouddomhof 3:dca57056e5cb 12 DigitalOut led_red(LED_RED);
arnouddomhof 3:dca57056e5cb 13 DigitalOut led_green(LED_GREEN);
arnouddomhof 3:dca57056e5cb 14 DigitalOut led_blue(LED_BLUE);
AppelSab 6:a02ad75f0333 15 // Buttons
arnouddomhof 9:8b2d6ec577e3 16 DigitalIn button_clbrt_home(SW2);
AppelSab 12:3e084e1a699e 17 DigitalIn button_Demo(D2);
AppelSab 12:3e084e1a699e 18 DigitalIn button_Emg(D3);
AppelSab 6:a02ad75f0333 19 DigitalIn Fail_button(SW3);
AppelSab 6:a02ad75f0333 20 // Modserial
arnouddomhof 3:dca57056e5cb 21 MODSERIAL pc(USBTX, USBRX);
AppelSab 6:a02ad75f0333 22 // Encoders
AppelSab 6:a02ad75f0333 23 QEI Encoder1(D11, D10, NC, 4200) ; // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
AppelSab 6:a02ad75f0333 24 QEI Encoder2(D9, D8, NC, 4200) ; // Encoder motor 2, (pin 2A, pin 2B, index pin (not used), counts/rev)
AppelSab 6:a02ad75f0333 25 // Motors (direction and PWM)
AppelSab 6:a02ad75f0333 26 DigitalOut directionM1(D4);
AppelSab 6:a02ad75f0333 27 DigitalOut directionM2(D7);
AppelSab 6:a02ad75f0333 28 FastPWM motor1_pwm(D5);
AppelSab 6:a02ad75f0333 29 FastPWM motor2_pwm(D6);
Mirjam 7:d4090f334ce2 30 // EMG input en start value of filtered EMG
Mirjam 7:d4090f334ce2 31 AnalogIn emg1_raw( A0 );
Mirjam 7:d4090f334ce2 32 AnalogIn emg2_raw( A1 );
Mirjam 7:d4090f334ce2 33 float threshold_EMG = 0.25; // Threshold on 25 percent of the maximum EMG
Mirjam 7:d4090f334ce2 34
AppelSab 6:a02ad75f0333 35 // Declare timers and Tickers
Mirjam 7:d4090f334ce2 36 Timer timer; // Timer for counting time in this state
Mirjam 7:d4090f334ce2 37 Ticker WriteValues; // Ticker to show values of velocity to screen
Mirjam 7:d4090f334ce2 38 Ticker StateMachine;
arnouddomhof 8:2afb66572fc4 39 //Ticker sample_EMGtoHIDscope; // Ticker to send the EMG signals to screen
arnouddomhof 8:2afb66572fc4 40 //HIDScope scope(4); //Number of channels which needs to be send to the HIDScope
AppelSab 12:3e084e1a699e 41 Ticker sample; // Ticker for reading out EMG
arnouddomhof 3:dca57056e5cb 42
AppelSab 6:a02ad75f0333 43 // Set up ProcessStateMachine
arnouddomhof 5:07e401cb251d 44 enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_DEMO, FAILURE_MODE};
arnouddomhof 3:dca57056e5cb 45 states currentState = WAITING;
AppelSab 6:a02ad75f0333 46 bool stateChanged = true;
AppelSab 6:a02ad75f0333 47
AppelSab 6:a02ad75f0333 48 // Global variables
arnouddomhof 3:dca57056e5cb 49 char c;
AppelSab 12:3e084e1a699e 50 const float fs = 1/1024;
AppelSab 6:a02ad75f0333 51 int counts1;
AppelSab 6:a02ad75f0333 52 int counts2;
AppelSab 6:a02ad75f0333 53 float theta1;
AppelSab 6:a02ad75f0333 54 float theta2;
AppelSab 6:a02ad75f0333 55 float vel_1;
AppelSab 6:a02ad75f0333 56 float vel_2;
AppelSab 6:a02ad75f0333 57 float theta1_prev = 0.0;
AppelSab 6:a02ad75f0333 58 float theta2_prev = 0.0;
AppelSab 6:a02ad75f0333 59 const float pi = 3.14159265359;
AppelSab 6:a02ad75f0333 60 float tijd = 0.005;
AppelSab 6:a02ad75f0333 61 float time_in_state;
Mirjam 7:d4090f334ce2 62 int need_to_move_1; // Does the robot needs to move in the first direction?
Mirjam 7:d4090f334ce2 63 int need_to_move_2; // Does the robot needs to move in the second direction?
AppelSab 12:3e084e1a699e 64 volatile double EMG_calibrated_max_1 = 0.00000; // Maximum value of the first EMG signal found in the calibration state.
AppelSab 12:3e084e1a699e 65 volatile double EMG_calibrated_max_2 = 0.00000; // Maximum value of the second EMG signal found in the calibration state.
AppelSab 12:3e084e1a699e 66 volatile double emg1_cal; //measured value of the first emg
AppelSab 12:3e084e1a699e 67 volatile double emg2_cal; //measured value of the second emg
AppelSab 12:3e084e1a699e 68 const double x0 = 80.0; //zero x position after homing
AppelSab 12:3e084e1a699e 69 const double y0 = 141.0; //zero y position after homing
AppelSab 12:3e084e1a699e 70 volatile double setpointx = x0;
AppelSab 12:3e084e1a699e 71 volatile double setpointy = y0;
AppelSab 12:3e084e1a699e 72 volatile int sx;//value of the button and store as switch
AppelSab 12:3e084e1a699e 73 volatile int sy;//value of the button and store as switch
AppelSab 12:3e084e1a699e 74 double dirx = 1.0; //determine the direction of the setpoint placement
AppelSab 12:3e084e1a699e 75 double diry = 1.0; //determine the direction of the setpoint placement
AppelSab 12:3e084e1a699e 76 volatile double U1;
AppelSab 12:3e084e1a699e 77 volatile double U2;
AppelSab 12:3e084e1a699e 78
AppelSab 12:3e084e1a699e 79 // Inverse Kinematics
AppelSab 12:3e084e1a699e 80 volatile double q1_diff;
AppelSab 12:3e084e1a699e 81 volatile double q2_diff;
AppelSab 12:3e084e1a699e 82 const double sq = 2.0; //to square numbers
AppelSab 12:3e084e1a699e 83 const double L1 = 250.0; //length of the first link
AppelSab 12:3e084e1a699e 84 const double L3 = 350.0; //length of the second link
AppelSab 12:3e084e1a699e 85
AppelSab 12:3e084e1a699e 86 // Reference angles of the starting position
AppelSab 12:3e084e1a699e 87 double q2_0 = -acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
AppelSab 12:3e084e1a699e 88 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))));
AppelSab 12:3e084e1a699e 89 double q2_0_enc = (pi-q2_0) - q1_0;
AppelSab 12:3e084e1a699e 90
AppelSab 12:3e084e1a699e 91 // DEMO
AppelSab 12:3e084e1a699e 92 double point1x = 200.0;
AppelSab 12:3e084e1a699e 93 double point1y = 200.0;
AppelSab 12:3e084e1a699e 94 double point2x = 350.0;
AppelSab 12:3e084e1a699e 95 double point2y = 200.0;
AppelSab 12:3e084e1a699e 96 double point3x = 350.0;
AppelSab 12:3e084e1a699e 97 double point3y = 0;
AppelSab 12:3e084e1a699e 98 double point4x = 200.0;
AppelSab 12:3e084e1a699e 99 double point4y = 0;
AppelSab 12:3e084e1a699e 100 volatile int track = 1;
AppelSab 12:3e084e1a699e 101
AppelSab 12:3e084e1a699e 102 // Determine demo setpoints
AppelSab 12:3e084e1a699e 103 const double stepsize1 = 0.15;
AppelSab 12:3e084e1a699e 104 const double stepsize2 = 0.25;
AppelSab 12:3e084e1a699e 105 const double setpoint_error = 0.3;
Mirjam 7:d4090f334ce2 106
AppelSab 6:a02ad75f0333 107 // ----------------------------------------------
AppelSab 6:a02ad75f0333 108 // ------- FUNCTIONS ----------------------------
AppelSab 6:a02ad75f0333 109 // ----------------------------------------------
AppelSab 6:a02ad75f0333 110
AppelSab 12:3e084e1a699e 111 // Encoders
AppelSab 12:3e084e1a699e 112 void 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 }
AppelSab 12:3e084e1a699e 119 void ReadEncoder2() // Read encoder of motor 2.
AppelSab 6:a02ad75f0333 120 {
AppelSab 6:a02ad75f0333 121 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
AppelSab 6:a02ad75f0333 122 theta2 = (float(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
AppelSab 6:a02ad75f0333 123 vel_2 = (theta2 - theta2_prev) / tijd; // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
AppelSab 6:a02ad75f0333 124 theta2_prev = theta2; // Define theta_prev
AppelSab 6:a02ad75f0333 125 }
AppelSab 12:3e084e1a699e 126
AppelSab 12:3e084e1a699e 127 // Motor calibration
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 }
AppelSab 12:3e084e1a699e 139
AppelSab 12:3e084e1a699e 140 // Read EMG
AppelSab 12:3e084e1a699e 141 void ReadEMG()
AppelSab 12:3e084e1a699e 142 {
AppelSab 12:3e084e1a699e 143 emg1_cal = FilterDesign(emg1_raw.read());
AppelSab 12:3e084e1a699e 144 emg2_cal = FilterDesign2(emg2_raw.read());
AppelSab 12:3e084e1a699e 145 pc.printf("emg1_cal = %g, emg2_cal = %g \n\r", emg1_cal, emg2_cal);
AppelSab 12:3e084e1a699e 146 }
AppelSab 12:3e084e1a699e 147
AppelSab 12:3e084e1a699e 148 // EMG calibration
AppelSab 12:3e084e1a699e 149 void EMG_calibration()
AppelSab 12:3e084e1a699e 150 {
AppelSab 12:3e084e1a699e 151
AppelSab 12:3e084e1a699e 152 for (int i = 0; i <= 10; i++) //10 measuring points
AppelSab 12:3e084e1a699e 153 {
AppelSab 12:3e084e1a699e 154 ReadEMG();
AppelSab 12:3e084e1a699e 155 if (emg1_cal > EMG_calibrated_max_1){
AppelSab 12:3e084e1a699e 156 EMG_calibrated_max_1 = emg1_cal;}
AppelSab 12:3e084e1a699e 157
AppelSab 12:3e084e1a699e 158 if (emg2_cal > EMG_calibrated_max_2){
AppelSab 12:3e084e1a699e 159 EMG_calibrated_max_2 = emg2_cal;}
AppelSab 12:3e084e1a699e 160
AppelSab 12:3e084e1a699e 161 pc.printf("EMG1_max = %f, EMG2_max = %f \r\nEMG1_filtered = %f \r\nEMG2_filtered = %f \r\n",EMG_calibrated_max_1,EMG_calibrated_max_2, emg1_cal, emg2_cal);
AppelSab 12:3e084e1a699e 162 wait(0.5f);
AppelSab 12:3e084e1a699e 163 }
AppelSab 12:3e084e1a699e 164 }
AppelSab 12:3e084e1a699e 165
AppelSab 12:3e084e1a699e 166 // Inverse Kinematics
AppelSab 12:3e084e1a699e 167 double makeAngleq1(double x, double y){
AppelSab 12:3e084e1a699e 168 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
AppelSab 12:3e084e1a699e 169 q1_diff = -(2.0*(q1-q1_0)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
AppelSab 12:3e084e1a699e 170 return q1_diff;
AppelSab 12:3e084e1a699e 171 }
AppelSab 12:3e084e1a699e 172
AppelSab 12:3e084e1a699e 173 double makeAngleq2(double x, double y){
AppelSab 12:3e084e1a699e 174 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
AppelSab 12:3e084e1a699e 175 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
AppelSab 12:3e084e1a699e 176 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
AppelSab 12:3e084e1a699e 177 q2_diff = (2.0*(q2_motor - q2_0_enc)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
AppelSab 12:3e084e1a699e 178 return q2_diff;
AppelSab 12:3e084e1a699e 179 }
AppelSab 12:3e084e1a699e 180
AppelSab 12:3e084e1a699e 181 // PI controllers
AppelSab 12:3e084e1a699e 182 double PI_controller1(double error1)
Mirjam 7:d4090f334ce2 183 {
AppelSab 12:3e084e1a699e 184 static double error_integral1 = 0;
AppelSab 12:3e084e1a699e 185 static double error_prev1 = error1; // initialization with this value only done once!
AppelSab 12:3e084e1a699e 186
AppelSab 12:3e084e1a699e 187 // Proportional part
AppelSab 12:3e084e1a699e 188 double Kp1 = 20.0; // Kp (proportionele controller, nu nog een random waarde)
AppelSab 12:3e084e1a699e 189 double u_p1 = Kp1*error1; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
AppelSab 12:3e084e1a699e 190
AppelSab 12:3e084e1a699e 191 // Integral part
AppelSab 12:3e084e1a699e 192 double Ki1 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde)
AppelSab 12:3e084e1a699e 193 double Ts1 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
AppelSab 12:3e084e1a699e 194 error_integral1 = error_integral1 + error1 * Ts1;
AppelSab 12:3e084e1a699e 195 double u_i1 = Ki1 * error_integral1;
AppelSab 12:3e084e1a699e 196
AppelSab 12:3e084e1a699e 197 // Derivative part
AppelSab 12:3e084e1a699e 198 double Kd1 = 2.0;
AppelSab 12:3e084e1a699e 199 double error_derivative1 = (error1 - error_prev1)/Ts1;
AppelSab 12:3e084e1a699e 200 double u_d1 = Kd1 * error_derivative1;
AppelSab 12:3e084e1a699e 201 error_prev1 = error1;
AppelSab 12:3e084e1a699e 202
AppelSab 12:3e084e1a699e 203 // Sum
AppelSab 12:3e084e1a699e 204 U1 = u_p1 + u_i1 + u_d1;
AppelSab 12:3e084e1a699e 205
AppelSab 12:3e084e1a699e 206 // Return
AppelSab 12:3e084e1a699e 207 return U1;
AppelSab 12:3e084e1a699e 208 }
AppelSab 12:3e084e1a699e 209 double PI_controller2(double error2)
AppelSab 12:3e084e1a699e 210 {
AppelSab 12:3e084e1a699e 211 static double error_integral2 = 0;
AppelSab 12:3e084e1a699e 212 static double error_prev2 = error2; // initialization with this value only done once!
AppelSab 12:3e084e1a699e 213
AppelSab 12:3e084e1a699e 214 // Proportional part
AppelSab 12:3e084e1a699e 215 double Kp2 = 20.0; // Kp (proportionele controller, nu nog een random waarde)
AppelSab 12:3e084e1a699e 216 double u_p2 = Kp2*error2; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
AppelSab 12:3e084e1a699e 217
AppelSab 12:3e084e1a699e 218 // Integral part
AppelSab 12:3e084e1a699e 219 double Ki2 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde)
AppelSab 12:3e084e1a699e 220 double Ts2 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
AppelSab 12:3e084e1a699e 221 error_integral2 = error_integral2 + error2 * Ts2;
AppelSab 12:3e084e1a699e 222 double u_i2 = Ki2 * error_integral2;
AppelSab 12:3e084e1a699e 223
AppelSab 12:3e084e1a699e 224 // Derivative part
AppelSab 12:3e084e1a699e 225 double Kd2 = 2.0;
AppelSab 12:3e084e1a699e 226 double error_derivative2 = (error2 - error_prev2)/Ts2;
AppelSab 12:3e084e1a699e 227 double u_d2 = Kd2 * error_derivative2;
AppelSab 12:3e084e1a699e 228 error_prev2 = error2;
Mirjam 7:d4090f334ce2 229
AppelSab 12:3e084e1a699e 230 // Sum +
AppelSab 12:3e084e1a699e 231 U2 = u_p2 + u_i2 + u_d2;
AppelSab 12:3e084e1a699e 232
AppelSab 12:3e084e1a699e 233 // Return
AppelSab 12:3e084e1a699e 234 return U2;
AppelSab 12:3e084e1a699e 235 }
AppelSab 12:3e084e1a699e 236
AppelSab 12:3e084e1a699e 237 // Determination of setpoint
AppelSab 12:3e084e1a699e 238 void determineEMGset(){
AppelSab 12:3e084e1a699e 239 const double v = 0.1; //moving speed of setpoint
AppelSab 12:3e084e1a699e 240 setpointx = setpointx + dirx*sx*v;
AppelSab 12:3e084e1a699e 241 setpointy = setpointy + diry*sy*v;
AppelSab 12:3e084e1a699e 242 }
AppelSab 12:3e084e1a699e 243 void ChangeDirectionX(){
AppelSab 12:3e084e1a699e 244 dirx = -1*dirx;
AppelSab 12:3e084e1a699e 245 }
AppelSab 12:3e084e1a699e 246 void ChangeDirectionY(){
AppelSab 12:3e084e1a699e 247 diry = -1*diry;
AppelSab 12:3e084e1a699e 248 }
AppelSab 12:3e084e1a699e 249
AppelSab 12:3e084e1a699e 250 // Motoraansturing voor EMG signalen
AppelSab 12:3e084e1a699e 251 /**
AppelSab 12:3e084e1a699e 252 void motoraansturing()
AppelSab 12:3e084e1a699e 253 {
AppelSab 12:3e084e1a699e 254 determineEMGset();
AppelSab 12:3e084e1a699e 255 q1_diff = makeAngleq1(setpointx, setpointy);
AppelSab 12:3e084e1a699e 256 q2_diff = makeAngleq2(setpointx, setpointy);
AppelSab 12:3e084e1a699e 257 ReadEncoder1();
AppelSab 12:3e084e1a699e 258 ReadEncoder2();
AppelSab 12:3e084e1a699e 259 double error2 = q2_diff - theta2;
AppelSab 12:3e084e1a699e 260 double error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as.
AppelSab 12:3e084e1a699e 261 U1 = PI_controller1(error1); // Voltage dat naar de motor gestuurd wordt.
AppelSab 12:3e084e1a699e 262 U2 = PI_controller2(error2);
AppelSab 12:3e084e1a699e 263 pc.printf("U1 = %g, U2 = %g \n\r", U1, U2);
AppelSab 12:3e084e1a699e 264 motor1_pwm.write(fabs(U1)); // Motor aansturen
AppelSab 12:3e084e1a699e 265 directionM1 = U1 > 0.0f; // Richting van de motor bepalen
AppelSab 12:3e084e1a699e 266 motor2_pwm.write(fabs(U2));
AppelSab 12:3e084e1a699e 267 directionM2 = U2 > 0.0f;
Mirjam 7:d4090f334ce2 268 }
AppelSab 12:3e084e1a699e 269 **/
AppelSab 12:3e084e1a699e 270 double determinedemosetx(double setpointx, double setpointy)
AppelSab 12:3e084e1a699e 271 {
AppelSab 12:3e084e1a699e 272
AppelSab 12:3e084e1a699e 273 if (setpointx < point1x && track == 1){
AppelSab 12:3e084e1a699e 274 setpointx = setpointx + stepsize1;
AppelSab 12:3e084e1a699e 275 }
AppelSab 12:3e084e1a699e 276
AppelSab 12:3e084e1a699e 277 // Van punt 1 naar punt 2.
AppelSab 12:3e084e1a699e 278 if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 41)) {
AppelSab 12:3e084e1a699e 279 track = 12;
AppelSab 12:3e084e1a699e 280 }
AppelSab 12:3e084e1a699e 281
AppelSab 12:3e084e1a699e 282 if (setpointx < point2x && track == 12){
AppelSab 12:3e084e1a699e 283 setpointx = setpointx + stepsize2;
AppelSab 12:3e084e1a699e 284 }
AppelSab 12:3e084e1a699e 285
AppelSab 12:3e084e1a699e 286 // Van punt 2 naar punt 3.
AppelSab 12:3e084e1a699e 287 if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && track == 12){
AppelSab 12:3e084e1a699e 288 setpointx = point3x;
AppelSab 12:3e084e1a699e 289 track = 23;
AppelSab 12:3e084e1a699e 290 }
AppelSab 12:3e084e1a699e 291
AppelSab 12:3e084e1a699e 292 if (setpointy > point3y && track == 23){
AppelSab 12:3e084e1a699e 293 setpointx = point3x; // Van punt 1 naar punt 2 op dezelfde x blijven.
AppelSab 12:3e084e1a699e 294 }
AppelSab 12:3e084e1a699e 295
AppelSab 12:3e084e1a699e 296 // Van punt 3 naar punt 4.
AppelSab 12:3e084e1a699e 297 if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)) {
AppelSab 12:3e084e1a699e 298 setpointy = point4y;
AppelSab 12:3e084e1a699e 299 track = 34;
AppelSab 12:3e084e1a699e 300 }
AppelSab 12:3e084e1a699e 301
AppelSab 12:3e084e1a699e 302 if (setpointx > point4x && track == 34){
AppelSab 12:3e084e1a699e 303 setpointx = setpointx - stepsize2;
AppelSab 12:3e084e1a699e 304 }
AppelSab 12:3e084e1a699e 305
AppelSab 12:3e084e1a699e 306 // Van punt 4 naar punt 1.
AppelSab 12:3e084e1a699e 307 if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34)){
AppelSab 12:3e084e1a699e 308 setpointx = point4x;
AppelSab 12:3e084e1a699e 309 track = 41;
AppelSab 12:3e084e1a699e 310 }
AppelSab 12:3e084e1a699e 311
AppelSab 12:3e084e1a699e 312 if (setpointy < point1y && track == 41){
AppelSab 12:3e084e1a699e 313 setpointx = point4x; // Van punt 4 naar punt 2 op dezelfde x blijven.
AppelSab 12:3e084e1a699e 314 }
AppelSab 12:3e084e1a699e 315 return setpointx;
AppelSab 12:3e084e1a699e 316 }
AppelSab 12:3e084e1a699e 317
AppelSab 12:3e084e1a699e 318 double determinedemosety(double setpointx, double setpointy)
AppelSab 12:3e084e1a699e 319 {
AppelSab 12:3e084e1a699e 320 // Van reference positie naar punt 1.
AppelSab 12:3e084e1a699e 321 if(setpointy < point1y && track == 1){
AppelSab 12:3e084e1a699e 322 setpointy = setpointy + (stepsize2);
AppelSab 12:3e084e1a699e 323 }
AppelSab 12:3e084e1a699e 324
AppelSab 12:3e084e1a699e 325 // Van punt 1 naar punt 2.
AppelSab 12:3e084e1a699e 326 if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 41)){
AppelSab 12:3e084e1a699e 327 setpointy = point2y; // Van punt 1 naar punt 2 op dezelfde y blijven.
AppelSab 12:3e084e1a699e 328 track = 12;
AppelSab 12:3e084e1a699e 329 }
AppelSab 12:3e084e1a699e 330 if (setpointx < point2x && track == 12){
AppelSab 12:3e084e1a699e 331 setpointy = point2y;
AppelSab 12:3e084e1a699e 332 }
AppelSab 12:3e084e1a699e 333
AppelSab 12:3e084e1a699e 334 // Van punt 2 naar punt 3.
AppelSab 12:3e084e1a699e 335 if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && (track == 12)){
AppelSab 12:3e084e1a699e 336 setpointx = point3x;
AppelSab 12:3e084e1a699e 337 track = 23;
AppelSab 12:3e084e1a699e 338 }
AppelSab 12:3e084e1a699e 339 if ((setpointy > point3y) && (track == 23)){
AppelSab 12:3e084e1a699e 340 setpointy = setpointy + (-stepsize2);
AppelSab 12:3e084e1a699e 341 track = 23;
AppelSab 12:3e084e1a699e 342 }
AppelSab 12:3e084e1a699e 343
AppelSab 12:3e084e1a699e 344 // Van punt 3 naar punt 4.
AppelSab 12:3e084e1a699e 345 if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)){
AppelSab 12:3e084e1a699e 346 setpointy = setpointy;
AppelSab 12:3e084e1a699e 347 track = 34;
AppelSab 12:3e084e1a699e 348 }
AppelSab 12:3e084e1a699e 349 if (setpointx > point4x && track == 34){
AppelSab 12:3e084e1a699e 350 setpointy = setpointy;
AppelSab 12:3e084e1a699e 351 }
AppelSab 12:3e084e1a699e 352
AppelSab 12:3e084e1a699e 353 // Van punt 4 naar punt 1.
AppelSab 12:3e084e1a699e 354 if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34)){
AppelSab 12:3e084e1a699e 355 track = 41;
AppelSab 12:3e084e1a699e 356 }
AppelSab 12:3e084e1a699e 357
AppelSab 12:3e084e1a699e 358 if (setpointy < point1y && track == 41){
AppelSab 12:3e084e1a699e 359 setpointy = setpointy + (stepsize2); // Van punt 4 naar punt 2 op dezelfde x blijven.
AppelSab 12:3e084e1a699e 360 }
AppelSab 12:3e084e1a699e 361 return setpointy;
AppelSab 12:3e084e1a699e 362
AppelSab 12:3e084e1a699e 363 }
AppelSab 12:3e084e1a699e 364 void motoraansturingdemo()
AppelSab 12:3e084e1a699e 365 {
AppelSab 12:3e084e1a699e 366 setpointx = determinedemosetx(setpointx, setpointy);
AppelSab 12:3e084e1a699e 367 setpointy = determinedemosety(setpointx, setpointy);
AppelSab 12:3e084e1a699e 368 q1_diff = makeAngleq1(setpointx, setpointy);
AppelSab 12:3e084e1a699e 369 q2_diff = makeAngleq2(setpointx, setpointy);
AppelSab 12:3e084e1a699e 370
AppelSab 12:3e084e1a699e 371 ReadEncoder1();
AppelSab 12:3e084e1a699e 372 ReadEncoder2();
AppelSab 12:3e084e1a699e 373 double error2 = q2_diff - theta2;
AppelSab 12:3e084e1a699e 374 double error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as.
AppelSab 12:3e084e1a699e 375
AppelSab 12:3e084e1a699e 376 U1 = PI_controller1(error1); // Voltage dat naar de motor gestuurd wordt.
AppelSab 12:3e084e1a699e 377 U2 = PI_controller2(error2);
AppelSab 12:3e084e1a699e 378
AppelSab 12:3e084e1a699e 379 motor1_pwm.write(fabs(U1)); // Motor aansturen
AppelSab 12:3e084e1a699e 380 directionM1 = U1 > 0.0f; // Richting van de motor bepalen
AppelSab 12:3e084e1a699e 381 motor2_pwm.write(fabs(U2));
AppelSab 12:3e084e1a699e 382 directionM2 = U2 > 0.0f;
AppelSab 12:3e084e1a699e 383 }
AppelSab 6:a02ad75f0333 384 // ---------------------------------------------------
AppelSab 6:a02ad75f0333 385 // --------STATEMACHINE-------------------------------
AppelSab 6:a02ad75f0333 386 // ---------------------------------------------------
AppelSab 6:a02ad75f0333 387 void ProcessStateMachine(void)
AppelSab 6:a02ad75f0333 388 {
AppelSab 6:a02ad75f0333 389 switch (currentState)
AppelSab 6:a02ad75f0333 390 {
AppelSab 6:a02ad75f0333 391 case WAITING:
AppelSab 6:a02ad75f0333 392 // Description:
AppelSab 6:a02ad75f0333 393 // In this state we do nothing, and wait for a command
AppelSab 6:a02ad75f0333 394
AppelSab 6:a02ad75f0333 395 // Actions
AppelSab 6:a02ad75f0333 396 led_red = 0; led_green = 0; led_blue = 0; // Colouring the led WHITE
AppelSab 6:a02ad75f0333 397
AppelSab 6:a02ad75f0333 398 // State transition logic
arnouddomhof 9:8b2d6ec577e3 399 if (button_clbrt_home == 0)
AppelSab 6:a02ad75f0333 400 {
AppelSab 6:a02ad75f0333 401 currentState = MOTOR_ANGLE_CLBRT;
AppelSab 6:a02ad75f0333 402 stateChanged = true;
AppelSab 6:a02ad75f0333 403 pc.printf("Starting Calibration\n\r");
AppelSab 6:a02ad75f0333 404 }
arnouddomhof 9:8b2d6ec577e3 405 else if (Fail_button == 0)
AppelSab 6:a02ad75f0333 406 {
AppelSab 6:a02ad75f0333 407 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 408 stateChanged = true;
AppelSab 6:a02ad75f0333 409 }
AppelSab 6:a02ad75f0333 410 break;
AppelSab 6:a02ad75f0333 411
AppelSab 6:a02ad75f0333 412 case MOTOR_ANGLE_CLBRT:
AppelSab 6:a02ad75f0333 413 // Description:
AppelSab 6:a02ad75f0333 414 // In this state the robot moves with low motor PWM to some
AppelSab 6:a02ad75f0333 415 // mechanical limit of motion, in order to calibrate the motors.
AppelSab 6:a02ad75f0333 416
AppelSab 6:a02ad75f0333 417 // Actions
AppelSab 6:a02ad75f0333 418 led_red = 1; led_green = 0; led_blue = 0; // Colouring the led TURQUOISE
AppelSab 6:a02ad75f0333 419 timer.start(); //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT"
AppelSab 6:a02ad75f0333 420 if (stateChanged)
AppelSab 6:a02ad75f0333 421 {
AppelSab 6:a02ad75f0333 422 MotorAngleCalibrate(); // Actuate motor 1 and 2.
AppelSab 12:3e084e1a699e 423 ReadEncoder1(); // Get velocity of motor 1
AppelSab 12:3e084e1a699e 424 ReadEncoder2(); // Get velocity of motor 2
AppelSab 6:a02ad75f0333 425 stateChanged = true; // Keep this loop going, until the transition conditions are satisfied.
AppelSab 6:a02ad75f0333 426 }
AppelSab 6:a02ad75f0333 427
AppelSab 6:a02ad75f0333 428 // State transition logic
AppelSab 6:a02ad75f0333 429 time_in_state = timer.read(); // Determine if this state has run for long enough.
arnouddomhof 3:dca57056e5cb 430
AppelSab 6:a02ad75f0333 431 if(time_in_state > 2.0f && vel_1 < 1.1f && vel_2 < 1.1f)
AppelSab 6:a02ad75f0333 432 {
AppelSab 6:a02ad75f0333 433 //pc.printf( "Tijd in deze staat = %f \n\r", time_in_state);
AppelSab 6:a02ad75f0333 434 //pc.printf( "Tijd tijdens actions loop (Waarde voor bepalen van snelheid)") = %f \n\r", tijd);
arnouddomhof 8:2afb66572fc4 435 pc.printf("Motor calibration has ended. \n\r");
AppelSab 6:a02ad75f0333 436 timer.stop(); // Stop timer for this state
AppelSab 6:a02ad75f0333 437 timer.reset(); // Reset timer for this state
AppelSab 6:a02ad75f0333 438 motor1_pwm.write(fabs(0.0)); // Send PWM values to motor
AppelSab 6:a02ad75f0333 439 motor2_pwm.write(fabs(0.0));
AppelSab 6:a02ad75f0333 440 Encoder1.reset(); // Reset Encoders when arrived at zero-position
AppelSab 6:a02ad75f0333 441 Encoder2.reset();
AppelSab 6:a02ad75f0333 442
AppelSab 12:3e084e1a699e 443 currentState = HOMING; // Switch to next state (EMG_CLRBRT).
AppelSab 6:a02ad75f0333 444 stateChanged = true;
AppelSab 6:a02ad75f0333 445 }
AppelSab 6:a02ad75f0333 446 if (Fail_button == 0)
AppelSab 6:a02ad75f0333 447 {
AppelSab 6:a02ad75f0333 448 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 449 stateChanged = true;
AppelSab 6:a02ad75f0333 450 }
AppelSab 6:a02ad75f0333 451 break;
AppelSab 12:3e084e1a699e 452 /**
AppelSab 6:a02ad75f0333 453 case EMG_CLBRT:
AppelSab 6:a02ad75f0333 454 // In this state the person whom is connected to the robot needs
AppelSab 6:a02ad75f0333 455 // to flex his/her muscles as hard as possible, in order to
AppelSab 6:a02ad75f0333 456 // measure the maximum EMG-signal, which can be used to scale
AppelSab 6:a02ad75f0333 457 // the EMG-filter.
AppelSab 12:3e084e1a699e 458
AppelSab 12:3e084e1a699e 459
AppelSab 12:3e084e1a699e 460 led_red = 1; led_green = 1; led_blue = 0; // Colouring the led BLUE
AppelSab 12:3e084e1a699e 461
AppelSab 12:3e084e1a699e 462 // Actions
AppelSab 12:3e084e1a699e 463 if (stateChanged)
AppelSab 12:3e084e1a699e 464 {
AppelSab 12:3e084e1a699e 465 pc.printf("Starting EMG calibration. Contract muscles until the calibration is ended.\n\r");
AppelSab 12:3e084e1a699e 466 // motor1_pwm.write(fabs(0.0)); // Send PWM values to motor
AppelSab 12:3e084e1a699e 467 // motor2_pwm.write(fabs(0.0));
AppelSab 12:3e084e1a699e 468 EMG_calibration();
AppelSab 12:3e084e1a699e 469 pc.printf("Final: EMG1 = %f, EMG2 = %f \r\n",EMG_calibrated_max_1,EMG_calibrated_max_2);
AppelSab 12:3e084e1a699e 470 stateChanged = false;
Duif 10:3f93fdb90c29 471 }
Duif 10:3f93fdb90c29 472
AppelSab 12:3e084e1a699e 473 // State change logic
AppelSab 12:3e084e1a699e 474
AppelSab 12:3e084e1a699e 475 if (currentState == EMG_CLBRT && stateChanged == false){
AppelSab 12:3e084e1a699e 476 pc.printf("EMG calibration has ended. \n\r");
AppelSab 12:3e084e1a699e 477 currentState = WAITING4SIGNAL;
AppelSab 12:3e084e1a699e 478 stateChanged = true;
AppelSab 12:3e084e1a699e 479 }
AppelSab 6:a02ad75f0333 480 if (Fail_button == 0)
AppelSab 6:a02ad75f0333 481 {
AppelSab 6:a02ad75f0333 482 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 483 stateChanged = true;
AppelSab 12:3e084e1a699e 484 }
AppelSab 12:3e084e1a699e 485
AppelSab 12:3e084e1a699e 486 break;
AppelSab 12:3e084e1a699e 487 **/
AppelSab 12:3e084e1a699e 488
AppelSab 6:a02ad75f0333 489 case HOMING:
AppelSab 6:a02ad75f0333 490 // Description:
AppelSab 6:a02ad75f0333 491 // Robot moves to the home starting configuration
arnouddomhof 9:8b2d6ec577e3 492 pc.printf("HOMING \r\n");
AppelSab 6:a02ad75f0333 493 led_red = 0; led_green = 1; led_red = 0; // Colouring the led PURPLE
AppelSab 12:3e084e1a699e 494
AppelSab 12:3e084e1a699e 495 // Actions
AppelSab 12:3e084e1a699e 496 timer.start(); //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT"
AppelSab 12:3e084e1a699e 497 if (stateChanged)
AppelSab 12:3e084e1a699e 498 {
AppelSab 12:3e084e1a699e 499 MotorAngleCalibrate(); // Actuate motor 1 and 2.
AppelSab 12:3e084e1a699e 500 ReadEncoder1(); // Get velocity of motor 1
AppelSab 12:3e084e1a699e 501 ReadEncoder2(); // Get velocity of motor 2
AppelSab 12:3e084e1a699e 502 stateChanged = true; // Keep this loop going, until the transition conditions are satisfied.
AppelSab 12:3e084e1a699e 503 }
AppelSab 12:3e084e1a699e 504
AppelSab 12:3e084e1a699e 505 // State transition logic
AppelSab 12:3e084e1a699e 506 time_in_state = timer.read(); // Determine if this state has run for long enough.
AppelSab 12:3e084e1a699e 507 if(time_in_state > 2.0f && vel_1 < 1.1f && vel_2 < 1.1f)
AppelSab 12:3e084e1a699e 508 {
AppelSab 12:3e084e1a699e 509 pc.printf("Homing has ended. We are now in reference position. \n\r");
AppelSab 12:3e084e1a699e 510 timer.stop(); // Stop timer for this state
AppelSab 12:3e084e1a699e 511 timer.reset(); // Reset timer for this state
AppelSab 12:3e084e1a699e 512 motor1_pwm.write(fabs(0.0)); // Send PWM values to motor
AppelSab 12:3e084e1a699e 513 motor2_pwm.write(fabs(0.0));
AppelSab 12:3e084e1a699e 514 Encoder1.reset(); // Reset Encoders when arrived at zero-position
AppelSab 12:3e084e1a699e 515 Encoder2.reset();
AppelSab 12:3e084e1a699e 516
AppelSab 12:3e084e1a699e 517 currentState = WAITING4SIGNAL; // Switch to next state (EMG_CLRBRT).
AppelSab 12:3e084e1a699e 518 stateChanged = true;
AppelSab 12:3e084e1a699e 519 }
AppelSab 6:a02ad75f0333 520 if (Fail_button == 0)
AppelSab 12:3e084e1a699e 521 {
AppelSab 12:3e084e1a699e 522 currentState = FAILURE_MODE;
AppelSab 12:3e084e1a699e 523 stateChanged = true;
AppelSab 12:3e084e1a699e 524 }
AppelSab 6:a02ad75f0333 525 break;
AppelSab 12:3e084e1a699e 526
AppelSab 6:a02ad75f0333 527 case WAITING4SIGNAL:
AppelSab 6:a02ad75f0333 528 // Description:
AppelSab 6:a02ad75f0333 529 // In this state the robot waits for an action to occur.
arnouddomhof 9:8b2d6ec577e3 530
AppelSab 6:a02ad75f0333 531 led_red = 0; led_green = 0; led_blue = 0; // Colouring the led WHITE
AppelSab 6:a02ad75f0333 532
AppelSab 6:a02ad75f0333 533 // Requirements to move to the next state:
AppelSab 6:a02ad75f0333 534 // If a certain button is pressed we move to the corresponding
AppelSab 6:a02ad75f0333 535 // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
arnouddomhof 9:8b2d6ec577e3 536
arnouddomhof 9:8b2d6ec577e3 537 if (button_clbrt_home == 0)
AppelSab 6:a02ad75f0333 538 {
arnouddomhof 9:8b2d6ec577e3 539 currentState = MOTOR_ANGLE_CLBRT;
arnouddomhof 9:8b2d6ec577e3 540 stateChanged = true;
arnouddomhof 9:8b2d6ec577e3 541 pc.printf("Starting Calibration \n\r");
AppelSab 6:a02ad75f0333 542 }
arnouddomhof 9:8b2d6ec577e3 543 else if (button_Demo == 1)
AppelSab 6:a02ad75f0333 544 {
arnouddomhof 9:8b2d6ec577e3 545 currentState = MOVE_W_DEMO;
AppelSab 12:3e084e1a699e 546 stateChanged = true;
AppelSab 12:3e084e1a699e 547 pc.printf("DEMO mode \r\n");
arnouddomhof 9:8b2d6ec577e3 548 wait(1.0f);
AppelSab 6:a02ad75f0333 549 }
arnouddomhof 9:8b2d6ec577e3 550 else if (button_Emg == 1)
AppelSab 6:a02ad75f0333 551 {
arnouddomhof 9:8b2d6ec577e3 552 currentState = MOVE_W_EMG;
AppelSab 12:3e084e1a699e 553 stateChanged = true;
AppelSab 12:3e084e1a699e 554 pc.printf("EMG mode\r\n");
arnouddomhof 9:8b2d6ec577e3 555 wait(1.0f);
AppelSab 6:a02ad75f0333 556 }
arnouddomhof 9:8b2d6ec577e3 557 else if (Fail_button == 0)
AppelSab 6:a02ad75f0333 558 {
AppelSab 6:a02ad75f0333 559 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 560 stateChanged = true;
AppelSab 6:a02ad75f0333 561 }
arnouddomhof 9:8b2d6ec577e3 562
AppelSab 6:a02ad75f0333 563 break;
AppelSab 12:3e084e1a699e 564
AppelSab 12:3e084e1a699e 565
AppelSab 6:a02ad75f0333 566 case MOVE_W_DEMO:
AppelSab 6:a02ad75f0333 567 // Description:
AppelSab 6:a02ad75f0333 568 // In this state the robot follows a preprogrammed shape, e.g.
AppelSab 6:a02ad75f0333 569 // a square.
AppelSab 6:a02ad75f0333 570
AppelSab 6:a02ad75f0333 571 led_red = 1; led_green = 1; led_blue = 0; // Colouring the led GREEN
AppelSab 6:a02ad75f0333 572
AppelSab 6:a02ad75f0333 573 // Requirements to move to the next state:
AppelSab 6:a02ad75f0333 574 // When the home button or the failure button is pressed, we
AppelSab 6:a02ad75f0333 575 // will the move to the corresponding state.
AppelSab 6:a02ad75f0333 576
AppelSab 12:3e084e1a699e 577 // Actions
AppelSab 12:3e084e1a699e 578 if(stateChanged){
AppelSab 12:3e084e1a699e 579 motoraansturingdemo();
AppelSab 12:3e084e1a699e 580 stateChanged = true;
AppelSab 12:3e084e1a699e 581 }
arnouddomhof 9:8b2d6ec577e3 582
AppelSab 12:3e084e1a699e 583 // State transition
arnouddomhof 9:8b2d6ec577e3 584 if (button_clbrt_home == 0)
AppelSab 6:a02ad75f0333 585 {
arnouddomhof 9:8b2d6ec577e3 586 currentState = HOMING;
arnouddomhof 9:8b2d6ec577e3 587 stateChanged = true;
arnouddomhof 9:8b2d6ec577e3 588 pc.printf("Moving home\n\r");
AppelSab 6:a02ad75f0333 589 }
arnouddomhof 9:8b2d6ec577e3 590 else if (Fail_button == 0)
AppelSab 6:a02ad75f0333 591 {
AppelSab 6:a02ad75f0333 592 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 593 stateChanged = true;
AppelSab 6:a02ad75f0333 594 }
AppelSab 6:a02ad75f0333 595 break;
AppelSab 12:3e084e1a699e 596
AppelSab 12:3e084e1a699e 597 /**
AppelSab 6:a02ad75f0333 598 case MOVE_W_EMG:
AppelSab 6:a02ad75f0333 599 // Description:
AppelSab 6:a02ad75f0333 600 // In this state the robot will be controlled by use of
AppelSab 6:a02ad75f0333 601 // EMG-signals.
AppelSab 12:3e084e1a699e 602
AppelSab 12:3e084e1a699e 603 // Actions
AppelSab 6:a02ad75f0333 604 led_red = 1; led_green = 0; led_blue = 1; // Colouring the led GREEN
AppelSab 12:3e084e1a699e 605 ReadEMG();
AppelSab 12:3e084e1a699e 606 if (stateChanged){
AppelSab 12:3e084e1a699e 607 //ReadEMG();
AppelSab 12:3e084e1a699e 608 //pc.printf(" emg1 = %g, emg2 = %g \n\r ", emg1_cal, emg2_cal);
AppelSab 12:3e084e1a699e 609 if (emg1_cal >= (threshold_EMG*EMG_calibrated_max_1)){
AppelSab 12:3e084e1a699e 610 sx = 1; // The robot does have to move
Mirjam 7:d4090f334ce2 611 }
Mirjam 7:d4090f334ce2 612 else {
AppelSab 12:3e084e1a699e 613 sx = 0; // If the robot does not have to move
Mirjam 7:d4090f334ce2 614 }
Mirjam 7:d4090f334ce2 615
AppelSab 12:3e084e1a699e 616 if(emg1_cal >= threshold_EMG*EMG_calibrated_max_2){
AppelSab 12:3e084e1a699e 617 sy = 1;
Mirjam 7:d4090f334ce2 618 }
Mirjam 7:d4090f334ce2 619 else {
AppelSab 12:3e084e1a699e 620 sy = 0;
Mirjam 7:d4090f334ce2 621 }
AppelSab 12:3e084e1a699e 622
AppelSab 12:3e084e1a699e 623 motoraansturing();
AppelSab 12:3e084e1a699e 624 stateChanged = true;
AppelSab 12:3e084e1a699e 625 }
arnouddomhof 9:8b2d6ec577e3 626
AppelSab 12:3e084e1a699e 627 // State transition logic
arnouddomhof 9:8b2d6ec577e3 628 if (button_clbrt_home == 0)
AppelSab 6:a02ad75f0333 629 {
arnouddomhof 9:8b2d6ec577e3 630 currentState = MOTOR_ANGLE_CLBRT;
arnouddomhof 9:8b2d6ec577e3 631 stateChanged = true;
arnouddomhof 9:8b2d6ec577e3 632 pc.printf("Starting Calibration \n\r");
arnouddomhof 9:8b2d6ec577e3 633 }
arnouddomhof 9:8b2d6ec577e3 634 else if (Fail_button == 0)
AppelSab 6:a02ad75f0333 635 {
AppelSab 6:a02ad75f0333 636 currentState = FAILURE_MODE;
AppelSab 6:a02ad75f0333 637 stateChanged = true;
AppelSab 6:a02ad75f0333 638 }
AppelSab 12:3e084e1a699e 639 break;
AppelSab 12:3e084e1a699e 640 **/
AppelSab 12:3e084e1a699e 641
AppelSab 6:a02ad75f0333 642 case FAILURE_MODE:
AppelSab 6:a02ad75f0333 643 // Description:
AppelSab 6:a02ad75f0333 644 // This state is reached when the failure button is reached.
AppelSab 6:a02ad75f0333 645 // In this state everything is turned off.
AppelSab 6:a02ad75f0333 646
AppelSab 6:a02ad75f0333 647 led_red = 0; led_green = 1; led_blue = 1; // Colouring the led RED
AppelSab 6:a02ad75f0333 648 // Actions
AppelSab 6:a02ad75f0333 649 if (stateChanged)
AppelSab 6:a02ad75f0333 650 {
AppelSab 6:a02ad75f0333 651 motor1_pwm.write(fabs(0.0)); // Stop all motors!
AppelSab 6:a02ad75f0333 652 motor2_pwm.write(fabs(0.0));
AppelSab 6:a02ad75f0333 653 pc.printf("FAILURE MODE \r\n PLEASE RESTART THE WHOLE ROBOT \r\n (and make sure this does not happen again) \r\n");
AppelSab 6:a02ad75f0333 654 stateChanged = false;
AppelSab 6:a02ad75f0333 655 }
AppelSab 6:a02ad75f0333 656 break;
AppelSab 6:a02ad75f0333 657
AppelSab 6:a02ad75f0333 658 // State transition logic
AppelSab 6:a02ad75f0333 659 // No state transition, you need to restart the robot.
AppelSab 6:a02ad75f0333 660
AppelSab 6:a02ad75f0333 661 default:
AppelSab 6:a02ad75f0333 662 // This state is a default state, this state is reached when
AppelSab 6:a02ad75f0333 663 // the program somehow defies all of the other states.
AppelSab 6:a02ad75f0333 664
AppelSab 6:a02ad75f0333 665 pc.printf("Unknown or unimplemented state reached!!! \n\r");
AppelSab 6:a02ad75f0333 666 led_red = 1; led_green = 1; led_blue = 1; // Colouring the led BLACK
AppelSab 6:a02ad75f0333 667 for (int n = 0; n < 50; n++) // Making an SOS signal with the RED led
AppelSab 6:a02ad75f0333 668 {
AppelSab 6:a02ad75f0333 669 for (int i = 0; i < 6; i++)
AppelSab 6:a02ad75f0333 670 {
AppelSab 6:a02ad75f0333 671 led_red = !led_red;
AppelSab 6:a02ad75f0333 672 wait(0.6f);
AppelSab 6:a02ad75f0333 673 }
AppelSab 6:a02ad75f0333 674 wait(0.4f);
AppelSab 6:a02ad75f0333 675 for (int i = 0 ; i < 6; i++)
AppelSab 6:a02ad75f0333 676 {
AppelSab 6:a02ad75f0333 677 led_red = !led_red;
AppelSab 6:a02ad75f0333 678 wait(0.2f);
AppelSab 6:a02ad75f0333 679 }
AppelSab 6:a02ad75f0333 680 wait(0.4f);
AppelSab 12:3e084e1a699e 681 }
arnouddomhof 3:dca57056e5cb 682 }
AppelSab 12:3e084e1a699e 683
AppelSab 6:a02ad75f0333 684 }
AppelSab 12:3e084e1a699e 685
AppelSab 6:a02ad75f0333 686 // --------------------------------
AppelSab 6:a02ad75f0333 687 // ----- MAIN LOOP ----------------
AppelSab 6:a02ad75f0333 688 // --------------------------------
AppelSab 6:a02ad75f0333 689
Mirjam 0:46dbc9b620d8 690 int main()
Mirjam 0:46dbc9b620d8 691 {
Mirjam 4:a0c1c021026b 692 // Switch all LEDs off
arnouddomhof 3:dca57056e5cb 693 led_red = 1;
arnouddomhof 3:dca57056e5cb 694 led_green = 1;
arnouddomhof 3:dca57056e5cb 695 led_blue = 1;
AppelSab 6:a02ad75f0333 696
arnouddomhof 3:dca57056e5cb 697 pc.baud(115200);
arnouddomhof 8:2afb66572fc4 698
AppelSab 12:3e084e1a699e 699 pc.printf("\r\n _______________ FEED ME! _______________ \r\n");
arnouddomhof 8:2afb66572fc4 700 wait(0.5f);
arnouddomhof 8:2afb66572fc4 701 pc.printf("WAITING... \r\n");
arnouddomhof 8:2afb66572fc4 702
AppelSab 12:3e084e1a699e 703 //sample.attach(&ReadEMG, 0.02f);
AppelSab 6:a02ad75f0333 704 StateMachine.attach(&ProcessStateMachine, 0.005f); // Run statemachine 200 times per second
AppelSab 12:3e084e1a699e 705
AppelSab 12:3e084e1a699e 706
AppelSab 12:3e084e1a699e 707 InterruptIn directionx(SW3);
AppelSab 12:3e084e1a699e 708 directionx.fall(ChangeDirectionX); //change the direction of the setpoint in x direction
AppelSab 12:3e084e1a699e 709 InterruptIn directiony(SW2);
AppelSab 12:3e084e1a699e 710 directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
AppelSab 12:3e084e1a699e 711
Mirjam 0:46dbc9b620d8 712 while (true) {
AppelSab 6:a02ad75f0333 713 }
AppelSab 6:a02ad75f0333 714 }
AppelSab 6:a02ad75f0333 715
arnouddomhof 5:07e401cb251d 716