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@13:a2e281d5de89, 2018-11-01 (annotated)

Committer:
arnouddomhof
Date:
Thu Nov 01 19:39:06 2018 +0000
Revision:
13:a2e281d5de89
Parent:
12:3e084e1a699e
Child:
14:059fd8f6cbfd
Werkende demomode, motor calibration en homing

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