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@16:ac4e3730f61f, 2018-11-02 (annotated)

Committer:
Duif
Date:
Fri Nov 02 08:05:30 2018 +0000
Revision:
16:ac4e3730f61f
Parent:
15:40dd74bd48d1
Child:
21:8798f776f778
Werkende state machine, hopelijk robuuster gemaakt. Anders de versie van gisteravond gebruiken

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