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@14:059fd8f6cbfd, 2018-11-01 (annotated)

Committer:
arnouddomhof
Date:
Thu Nov 01 20:44:47 2018 +0000
Revision:
14:059fd8f6cbfd
Parent:
13:a2e281d5de89
Child:
15:40dd74bd48d1
Aansturing met knopjes erin gebouwd. Verder nog niet getest op de robot.

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