Henk van Gansen / Mbed 2 deprecated Project_BioRobotics

Vanmorgen aan gewerkt

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Project_BioRobotics by Jordi Luong

main.cpp@13:b455e2e8bed2, 2017-11-01 (annotated)

Committer:
SybrandBumaCDA
Date:
Wed Nov 01 11:12:48 2017 +0000
Revision:
13:b455e2e8bed2
Parent:
12:12b72bd60fd1 
1-11-2017 werkt ongeveer;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jordiluong 0:80ac024b84cb 1 #include "BiQuad.h"
jordiluong 0:80ac024b84cb 2 #include "FastPWM.h"
jordiluong 0:80ac024b84cb 3 #include "HIDScope.h"
jordiluong 5:0d3e8694726e 4 #include <math.h>
jordiluong 0:80ac024b84cb 5 #include "mbed.h"
jordiluong 5:0d3e8694726e 6 #include "MODSERIAL.h"
jordiluong 0:80ac024b84cb 7 #include "QEI.h"
jordiluong 0:80ac024b84cb 8
jordiluong 5:0d3e8694726e 9 const double pi = 3.1415926535897; // Definition of pi
jordiluong 5:0d3e8694726e 10
jordiluong 7:757e95b4dc46 11 // SERIAL COMMUNICATION WITH PC ////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 12 MODSERIAL pc(USBTX, USBRX);
jordiluong 12:12b72bd60fd1 13 HIDScope scope(4);
jordiluong 0:80ac024b84cb 14
jordiluong 7:757e95b4dc46 15 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 3:5c3edcd29448 16 enum states{MOTORS_OFF, MOVING, HITTING};
jordiluong 0:80ac024b84cb 17 states currentState = MOTORS_OFF; // Start with motors off
jordiluong 0:80ac024b84cb 18 bool stateChanged = true; // Make sure the initialization of first state is executed
jordiluong 0:80ac024b84cb 19
jordiluong 7:757e95b4dc46 20 // ENCODER /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 21 QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D10, ENC1B TO D11
jordiluong 5:0d3e8694726e 22 QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D12, ENC2B TO D13
jordiluong 4:ea7689bf97e1 23
jordiluong 7:757e95b4dc46 24 // PINS ////////////////////////////////////////////////////////////////////////
jordiluong 4:ea7689bf97e1 25 DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW
jordiluong 3:5c3edcd29448 26 PwmOut motor1MagnitudePin(D5);
jordiluong 3:5c3edcd29448 27 DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW?
jordiluong 3:5c3edcd29448 28 PwmOut motor2MagnitudePin(D6);
jordiluong 4:ea7689bf97e1 29 InterruptIn button1(D2); // CONNECT BUT1 TO D2
jordiluong 4:ea7689bf97e1 30 InterruptIn button2(D3); // CONNECT BUT2 TO D3
jordiluong 10:a9e344e440b8 31 InterruptIn button3(SW2);
jordiluong 10:a9e344e440b8 32 InterruptIn button4(SW3);
jordiluong 12:12b72bd60fd1 33 //AnalogIn emg(A0);
jordiluong 12:12b72bd60fd1 34 AnalogIn potmeter1(A0);
jordiluong 12:12b72bd60fd1 35 AnalogIn potmeter2(A1);
jordiluong 3:5c3edcd29448 36
jordiluong 7:757e95b4dc46 37 // MOTOR CONTROL ///////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 38 Ticker controllerTicker;
jordiluong 12:12b72bd60fd1 39 const double controller_Ts = 1/200.1; // Time step for controllerTicker [s]; Should be between 5kHz and 10kHz
jordiluong 5:0d3e8694726e 40 const double motorRatio = 131.25; // Ratio of the gearbox in the motors []
jordiluong 7:757e95b4dc46 41 const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse]
jordiluong 11:5c06c97c3673 42 double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start
SybrandBumaCDA 13:b455e2e8bed2 43 double velocity = 0.05; // X and Y velocity of the end effector when desired
jordiluong 7:757e95b4dc46 44
jordiluong 7:757e95b4dc46 45 // MOTOR 1
jordiluong 12:12b72bd60fd1 46 volatile double position1; // Position of motor 1 [rad]
jordiluong 12:12b72bd60fd1 47 volatile double reference1 = 0; // Desired rotation of motor 1 [rad]
jordiluong 12:12b72bd60fd1 48 double motor1Max = 0; // Maximum rotation of motor 1 [rad]
jordiluong 12:12b72bd60fd1 49 double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad]
jordiluong 5:0d3e8694726e 50 // Controller gains
SybrandBumaCDA 13:b455e2e8bed2 51 const double motor1_KP = 13; // Position gain []
SybrandBumaCDA 13:b455e2e8bed2 52 const double motor1_KI = 7; // Integral gain []
SybrandBumaCDA 13:b455e2e8bed2 53 const double motor1_KD = 0.3; // Derivative gain []
jordiluong 5:0d3e8694726e 54 double motor1_err_int = 0, motor1_prev_err = 0;
jordiluong 5:0d3e8694726e 55 // Derivative filter coefficients
jordiluong 12:12b72bd60fd1 56 const double motor1_f_a1 = 0.25, motor1_f_a2 = 0.8; // Derivative filter coefficients []
jordiluong 12:12b72bd60fd1 57 const double motor1_f_b0 = 1, motor1_f_b1 = 2, motor1_f_b2 = 0.8; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 58 // Filter variables
jordiluong 5:0d3e8694726e 59 double motor1_f_v1 = 0, motor1_f_v2 = 0;
jordiluong 7:757e95b4dc46 60
jordiluong 7:757e95b4dc46 61 // MOTOR 2
jordiluong 12:12b72bd60fd1 62 volatile double position2; // Position of motor 2 [rad]
jordiluong 10:a9e344e440b8 63 volatile double reference2 = 0; // Desired rotation of motor 2 [rad]
SybrandBumaCDA 13:b455e2e8bed2 64 double motor2Max = 2*pi*25/360; // Maximum rotation of motor 2 [rad]
SybrandBumaCDA 13:b455e2e8bed2 65 double motor2Min = 2*pi*-28/360; // Minimum rotation of motor 2 [rad]
jordiluong 5:0d3e8694726e 66 // Controller gains
SybrandBumaCDA 13:b455e2e8bed2 67 //const double motor2_KP = potmeter1*10; // Position gain []
SybrandBumaCDA 13:b455e2e8bed2 68 //const double motor2_KI = potmeter2*20; // Integral gain []
SybrandBumaCDA 13:b455e2e8bed2 69 const double motor2_KP = 13; // Position gain []
SybrandBumaCDA 13:b455e2e8bed2 70 const double motor2_KI = 5; // Integral gain []
SybrandBumaCDA 13:b455e2e8bed2 71 const double motor2_KD = 0.1; // Derivative gain []
jordiluong 10:a9e344e440b8 72 double motor2_err_int = 0, motor2_prev_err = 0;
jordiluong 5:0d3e8694726e 73 // Derivative filter coefficients
jordiluong 12:12b72bd60fd1 74 const double motor2_f_a1 = 0.25, motor2_f_a2 = 0.8; // Derivative filter coefficients []
jordiluong 12:12b72bd60fd1 75 const double motor2_f_b0 = 1, motor2_f_b1 = 2, motor2_f_b2 = 0.8; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 76 // Filter variables
jordiluong 10:a9e344e440b8 77 double motor2_f_v1 = 0, motor2_f_v2 = 0;
jordiluong 0:80ac024b84cb 78
jordiluong 7:757e95b4dc46 79 // EMG FILTER //////////////////////////////////////////////////////////////////
jordiluong 12:12b72bd60fd1 80 //BiQuadChain bqc;
jordiluong 12:12b72bd60fd1 81 //BiQuad bq1(3.6, 5.0, 2.0, 0.5, 30.0); // EMG filter coefficients []
jordiluong 12:12b72bd60fd1 82 //BiQuad bq2(0, 0, 0, 0, 0); // EMG filter coefficients []
jordiluong 12:12b72bd60fd1 83
jordiluong 12:12b72bd60fd1 84 Ticker sampleTicker;
jordiluong 12:12b72bd60fd1 85 const double tickerTs = 0.1; // Time step for sampling [s]
jordiluong 7:757e95b4dc46 86
jordiluong 7:757e95b4dc46 87
jordiluong 7:757e95b4dc46 88 // FUNCTIONS ///////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 89 // BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
jordiluong 5:0d3e8694726e 90 double biquad(double u, double &v1, double &v2, const double a1,
jordiluong 5:0d3e8694726e 91 const double a2, const double b0, const double b1, const double b2)
jordiluong 0:80ac024b84cb 92 {
jordiluong 5:0d3e8694726e 93 double v = u - a1*v1 - a2*v2;
jordiluong 5:0d3e8694726e 94 double y = b0*v + b1*v1 + b2*v2;
jordiluong 5:0d3e8694726e 95 v2 = v1; v1 = v;
jordiluong 5:0d3e8694726e 96 return y;
jordiluong 0:80ac024b84cb 97 }
jordiluong 0:80ac024b84cb 98
jordiluong 12:12b72bd60fd1 99
jordiluong 7:757e95b4dc46 100 // PID-CONTROLLER WITH FILTER //////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 101 double PID_Controller(double e, const double Kp, const double Ki,
jordiluong 5:0d3e8694726e 102 const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
jordiluong 5:0d3e8694726e 103 double &f_v2, const double f_a1, const double f_a2, const double f_b0,
jordiluong 5:0d3e8694726e 104 const double f_b1, const double f_b2)
jordiluong 0:80ac024b84cb 105 {
jordiluong 5:0d3e8694726e 106 // Derivative
jordiluong 8:78d8ccf84a38 107 double e_der = (e - e_prev)/Ts; // Calculate the derivative of error
jordiluong 12:12b72bd60fd1 108 e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); // Filter the derivative of error
jordiluong 12:12b72bd60fd1 109 //e_der = bq1.step(e_der);
jordiluong 5:0d3e8694726e 110 e_prev = e;
jordiluong 5:0d3e8694726e 111 // Integral
jordiluong 8:78d8ccf84a38 112 e_int = e_int + Ts*e; // Calculate the integral of error
jordiluong 5:0d3e8694726e 113 // PID
jordiluong 12:12b72bd60fd1 114 //pc.printf("%f --> %f \r\n", e_der, e_derf);
jordiluong 11:5c06c97c3673 115 return Kp*e + Ki*e_int + Kd*e_der;
jordiluong 3:5c3edcd29448 116 }
jordiluong 3:5c3edcd29448 117
jordiluong 7:757e95b4dc46 118 // MOTOR 1 /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 119 void RotateMotor1(double motor1Value)
jordiluong 3:5c3edcd29448 120 {
jordiluong 10:a9e344e440b8 121 if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
jordiluong 5:0d3e8694726e 122 {
jordiluong 10:a9e344e440b8 123 if(motor1Value >= 0) motor1DirectionPin = 0; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 124 else motor1DirectionPin = 1; // Rotate motor 1 CCW
jordiluong 5:0d3e8694726e 125
jordiluong 5:0d3e8694726e 126 if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
jordiluong 5:0d3e8694726e 127 else motor1MagnitudePin = fabs(motor1Value);
jordiluong 5:0d3e8694726e 128 }
jordiluong 5:0d3e8694726e 129 else motor1MagnitudePin = 0;
jordiluong 3:5c3edcd29448 130 }
jordiluong 0:80ac024b84cb 131
jordiluong 10:a9e344e440b8 132 // MOTOR 2 /////////////////////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 133 void RotateMotor2(double motor2Value)
jordiluong 10:a9e344e440b8 134 {
jordiluong 10:a9e344e440b8 135 if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
jordiluong 10:a9e344e440b8 136 {
jordiluong 10:a9e344e440b8 137 if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 138 else motor2DirectionPin = 0; // Rotate motor 1 CCW
jordiluong 10:a9e344e440b8 139
jordiluong 10:a9e344e440b8 140 if(fabs(motor2Value) > 1) motor2MagnitudePin = 1;
jordiluong 10:a9e344e440b8 141 else motor2MagnitudePin = fabs(motor2Value);
jordiluong 10:a9e344e440b8 142 }
jordiluong 10:a9e344e440b8 143 else motor2MagnitudePin = 0;
jordiluong 10:a9e344e440b8 144 }
jordiluong 10:a9e344e440b8 145
jordiluong 7:757e95b4dc46 146 // MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 147 void Motor1Controller()
jordiluong 5:0d3e8694726e 148 {
jordiluong 10:a9e344e440b8 149 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 12:12b72bd60fd1 150 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 12:12b72bd60fd1 151 //pc.printf("error %f \r\n", reference1 - position1);
jordiluong 5:0d3e8694726e 152 double motor1Value = PID_Controller(reference1 - position1, motor1_KP,
jordiluong 5:0d3e8694726e 153 motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err,
jordiluong 5:0d3e8694726e 154 motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
jordiluong 5:0d3e8694726e 155 motor1_f_b1, motor1_f_b2);
jordiluong 10:a9e344e440b8 156 //pc.printf("motor value %f \r\n",motor1Value);
jordiluong 5:0d3e8694726e 157 RotateMotor1(motor1Value);
jordiluong 10:a9e344e440b8 158 double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
jordiluong 10:a9e344e440b8 159 motor2_KI, motor2_KD, controller_Ts, motor2_err_int, motor2_prev_err,
jordiluong 10:a9e344e440b8 160 motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
jordiluong 10:a9e344e440b8 161 motor2_f_b1, motor2_f_b2);
jordiluong 10:a9e344e440b8 162 RotateMotor2(motor2Value);
jordiluong 10:a9e344e440b8 163 }
jordiluong 10:a9e344e440b8 164
jordiluong 12:12b72bd60fd1 165 // MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 12:12b72bd60fd1 166 /*void Motor2Controller()
jordiluong 12:12b72bd60fd1 167 {
jordiluong 12:12b72bd60fd1 168 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 12:12b72bd60fd1 169 double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
jordiluong 12:12b72bd60fd1 170 motor2_KI, motor2_KD, controller2_Ts, motor2_err_int, motor2_prev_err,
jordiluong 12:12b72bd60fd1 171 motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
jordiluong 12:12b72bd60fd1 172 motor2_f_b1, motor2_f_b2);
jordiluong 12:12b72bd60fd1 173 RotateMotor2(motor2Value);
jordiluong 12:12b72bd60fd1 174 }
jordiluong 12:12b72bd60fd1 175 */
jordiluong 7:757e95b4dc46 176 // TURN OFF MOTORS /////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 177 void TurnMotorsOff()
jordiluong 5:0d3e8694726e 178 {
jordiluong 5:0d3e8694726e 179 motor1MagnitudePin = 0;
jordiluong 5:0d3e8694726e 180 motor2MagnitudePin = 0;
jordiluong 5:0d3e8694726e 181 }
jordiluong 5:0d3e8694726e 182
jordiluong 10:a9e344e440b8 183 // DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 184 void jointVelocities()
jordiluong 10:a9e344e440b8 185 {
jordiluong 10:a9e344e440b8 186 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 10:a9e344e440b8 187 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 10:a9e344e440b8 188
jordiluong 12:12b72bd60fd1 189 if(!button1 && reference1 > motor1Min && reference2 < motor2Max) xVelocity = velocity;
jordiluong 12:12b72bd60fd1 190 else if(!button2 && reference1 < motor1Max && reference2 > motor2Min) xVelocity = -velocity;
jordiluong 10:a9e344e440b8 191 else xVelocity = 0;
jordiluong 10:a9e344e440b8 192
jordiluong 12:12b72bd60fd1 193 if(!button3 && reference2 < motor2Max && reference1 > motor2Min) yVelocity = velocity;
jordiluong 12:12b72bd60fd1 194 else if(!button4 && reference2 > motor2Min && reference1 > motor2Min) yVelocity = -velocity;
jordiluong 10:a9e344e440b8 195 else yVelocity = 0;
jordiluong 10:a9e344e440b8 196
jordiluong 10:a9e344e440b8 197 //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity);
jordiluong 10:a9e344e440b8 198
jordiluong 10:a9e344e440b8 199 // Construct Jacobian
jordiluong 10:a9e344e440b8 200 double q[4];
jordiluong 10:a9e344e440b8 201 q[0] = position1, q[1] = -position1;
jordiluong 10:a9e344e440b8 202 q[2] = position2, q[3] = -position2;
jordiluong 10:a9e344e440b8 203
jordiluong 10:a9e344e440b8 204 double T2[3]; // Second column of the jacobian
jordiluong 10:a9e344e440b8 205 double T3[3]; // Third column of the jacobian
jordiluong 10:a9e344e440b8 206 double T4[3]; // Fourth column of the jacobian
jordiluong 10:a9e344e440b8 207 double T1[6];
jordiluong 10:a9e344e440b8 208 static const signed char b_T1[3] = { 1, 0, 0 };
jordiluong 10:a9e344e440b8 209 double J_data[6];
jordiluong 10:a9e344e440b8 210
jordiluong 10:a9e344e440b8 211 T2[0] = 1.0;
jordiluong 10:a9e344e440b8 212 T2[1] = 0.365 * cos(q[0]);
jordiluong 10:a9e344e440b8 213 T2[2] = 0.365 * sin(q[0]);
jordiluong 10:a9e344e440b8 214 T3[0] = 1.0;
jordiluong 10:a9e344e440b8 215 T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
jordiluong 10:a9e344e440b8 216 q[0]) + q[1]);
jordiluong 10:a9e344e440b8 217 T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
jordiluong 10:a9e344e440b8 218 q[0]) + q[1]);
jordiluong 10:a9e344e440b8 219 T4[0] = 1.0;
jordiluong 10:a9e344e440b8 220 T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
jordiluong 10:a9e344e440b8 221 q[0]) + q[1])) + 0.265 * sin((q[0] + q[1]) + q[2]);
jordiluong 10:a9e344e440b8 222 T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
jordiluong 10:a9e344e440b8 223 q[0]) + q[1])) - 0.265 * cos((q[0] + q[1]) + q[2]);
jordiluong 10:a9e344e440b8 224
jordiluong 10:a9e344e440b8 225 for (int i = 0; i < 3; i++)
jordiluong 10:a9e344e440b8 226 {
jordiluong 10:a9e344e440b8 227 T1[i] = (double)b_T1[i] - T2[i];
jordiluong 10:a9e344e440b8 228 T1[3 + i] = T3[i] - T4[i];
jordiluong 10:a9e344e440b8 229 }
jordiluong 10:a9e344e440b8 230
jordiluong 10:a9e344e440b8 231 for (int i = 0; i < 2; i++)
jordiluong 10:a9e344e440b8 232 {
jordiluong 10:a9e344e440b8 233 for (int j = 0; j < 3; j++)
jordiluong 10:a9e344e440b8 234 {
jordiluong 10:a9e344e440b8 235 J_data[j + 3 * i] = T1[j + 3 * i];
jordiluong 10:a9e344e440b8 236 }
jordiluong 10:a9e344e440b8 237 }
jordiluong 10:a9e344e440b8 238
jordiluong 10:a9e344e440b8 239 // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration
jordiluong 10:a9e344e440b8 240 // Note: the matrices from now on will we constructed rowwise
jordiluong 10:a9e344e440b8 241 double Jvelocity[4];
jordiluong 10:a9e344e440b8 242 Jvelocity[0] = J_data[1];
jordiluong 10:a9e344e440b8 243 Jvelocity[1] = J_data[4];
jordiluong 10:a9e344e440b8 244 Jvelocity[2] = J_data[2];
jordiluong 10:a9e344e440b8 245 Jvelocity[3] = J_data[5];
jordiluong 10:a9e344e440b8 246
jordiluong 10:a9e344e440b8 247 // Creating the inverse Jacobian
jordiluong 10:a9e344e440b8 248 double Jvelocity_inv[4]; // The inverse matrix of the jacobian
jordiluong 10:a9e344e440b8 249 double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2]; // The determinant of the matrix
jordiluong 10:a9e344e440b8 250 Jvelocity_inv[0] = Jvelocity[3]/determ;
jordiluong 10:a9e344e440b8 251 Jvelocity_inv[1] = -Jvelocity[1]/determ;
jordiluong 10:a9e344e440b8 252 Jvelocity_inv[2] = -Jvelocity[2]/determ;
jordiluong 10:a9e344e440b8 253 Jvelocity_inv[3] = Jvelocity[0]/determ;
jordiluong 10:a9e344e440b8 254
jordiluong 10:a9e344e440b8 255 // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian
jordiluong 10:a9e344e440b8 256 double msh[2]; // This is the velocity the joints have to have
jordiluong 10:a9e344e440b8 257 msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1];
jordiluong 10:a9e344e440b8 258 msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3];
jordiluong 10:a9e344e440b8 259
jordiluong 12:12b72bd60fd1 260 if(reference1 + msh[0]*tickerTs > motor1Max) reference1 = motor1Max;
jordiluong 12:12b72bd60fd1 261 else if(reference1 + msh[0]*tickerTs < motor1Min) reference1 = motor1Min;
jordiluong 12:12b72bd60fd1 262 else reference1 = reference1 + msh[0]*tickerTs;
jordiluong 10:a9e344e440b8 263
jordiluong 12:12b72bd60fd1 264 if(reference2 + msh[1]*tickerTs > motor2Max) reference2 = motor2Max;
jordiluong 12:12b72bd60fd1 265 else if(reference2 + msh[1]*tickerTs < motor2Min) reference2 = motor2Min;
jordiluong 12:12b72bd60fd1 266 else reference2 = reference2 + msh[1]*tickerTs;
jordiluong 10:a9e344e440b8 267
jordiluong 12:12b72bd60fd1 268 //Motor1Controller(), Motor2Controller();
jordiluong 12:12b72bd60fd1 269 scope.set(0,reference1);
jordiluong 12:12b72bd60fd1 270 scope.set(1,position1);
jordiluong 12:12b72bd60fd1 271 scope.set(2,reference2);
jordiluong 12:12b72bd60fd1 272 scope.set(3,position2);
jordiluong 12:12b72bd60fd1 273 scope.send();
jordiluong 10:a9e344e440b8 274
jordiluong 10:a9e344e440b8 275 pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360);
jordiluong 10:a9e344e440b8 276 pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360);
jordiluong 12:12b72bd60fd1 277 //pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts);
jordiluong 10:a9e344e440b8 278 }
jordiluong 10:a9e344e440b8 279
jordiluong 10:a9e344e440b8 280 // EMG /////////////////////////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 281 void EmgSample()
jordiluong 10:a9e344e440b8 282 {
jordiluong 10:a9e344e440b8 283 if(currentState == MOVING)
jordiluong 10:a9e344e440b8 284 {
jordiluong 10:a9e344e440b8 285 //double emgFiltered = bqc.step(emg.read()); // Filter the EMG signal
jordiluong 10:a9e344e440b8 286 /*
jordiluong 10:a9e344e440b8 287 If EMG signal 1 --> xVelocity / yVelocity = velocity
jordiluong 11:5c06c97c3673 288 Else if EMG signal 2 --> xVelocity / yVelocity = -velocity
jordiluong 10:a9e344e440b8 289 Else xVelocity / yVelocity = 0
jordiluong 11:5c06c97c3673 290 If both signal 1 and 2 --> Switch
jordiluong 10:a9e344e440b8 291 */
jordiluong 10:a9e344e440b8 292 jointVelocities();
jordiluong 10:a9e344e440b8 293 }
jordiluong 10:a9e344e440b8 294 }
jordiluong 10:a9e344e440b8 295
jordiluong 7:757e95b4dc46 296 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 297 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 298 {
jordiluong 0:80ac024b84cb 299 switch(currentState)
jordiluong 0:80ac024b84cb 300 {
jordiluong 0:80ac024b84cb 301 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 302 {
jordiluong 0:80ac024b84cb 303 // State initialization
jordiluong 0:80ac024b84cb 304 if(stateChanged)
jordiluong 0:80ac024b84cb 305 {
jordiluong 12:12b72bd60fd1 306 //pc.printf("Entering MOTORS_OFF \r\n"
jordiluong 12:12b72bd60fd1 307 //"Press button 1 to enter MOVING \r\n");
jordiluong 5:0d3e8694726e 308 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 309 stateChanged = false;
jordiluong 0:80ac024b84cb 310 }
jordiluong 0:80ac024b84cb 311
jordiluong 0:80ac024b84cb 312 // Home command
jordiluong 4:ea7689bf97e1 313 if(!button1)
jordiluong 0:80ac024b84cb 314 {
jordiluong 0:80ac024b84cb 315 currentState = MOVING;
jordiluong 0:80ac024b84cb 316 stateChanged = true;
jordiluong 0:80ac024b84cb 317 break;
jordiluong 0:80ac024b84cb 318 }
jordiluong 4:ea7689bf97e1 319 break;
jordiluong 0:80ac024b84cb 320 }
jordiluong 0:80ac024b84cb 321
jordiluong 0:80ac024b84cb 322 case MOVING:
jordiluong 0:80ac024b84cb 323 {
jordiluong 0:80ac024b84cb 324 // State initialization
jordiluong 0:80ac024b84cb 325 if(stateChanged)
jordiluong 0:80ac024b84cb 326 {
jordiluong 12:12b72bd60fd1 327 //pc.printf("Entering MOVING \r\n"
jordiluong 12:12b72bd60fd1 328 //"Press button 2 to enter HITTING \r\n");
jordiluong 0:80ac024b84cb 329 stateChanged = false;
jordiluong 0:80ac024b84cb 330 }
jordiluong 0:80ac024b84cb 331
jordiluong 0:80ac024b84cb 332 // Hit command
jordiluong 10:a9e344e440b8 333 /*if(!button2)
jordiluong 0:80ac024b84cb 334 {
jordiluong 0:80ac024b84cb 335 currentState = HITTING;
jordiluong 0:80ac024b84cb 336 stateChanged = true;
jordiluong 0:80ac024b84cb 337 break;
jordiluong 0:80ac024b84cb 338 }
jordiluong 10:a9e344e440b8 339 */
jordiluong 4:ea7689bf97e1 340 break;
jordiluong 0:80ac024b84cb 341 }
jordiluong 0:80ac024b84cb 342
jordiluong 0:80ac024b84cb 343 case HITTING:
jordiluong 0:80ac024b84cb 344 {
jordiluong 0:80ac024b84cb 345 // State initialization
jordiluong 0:80ac024b84cb 346 if(stateChanged)
jordiluong 0:80ac024b84cb 347 {
jordiluong 12:12b72bd60fd1 348 //pc.printf("Entering HITTING \r\n");
jordiluong 5:0d3e8694726e 349 stateChanged = false;
jordiluong 4:ea7689bf97e1 350 //HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 351 currentState = MOVING;
jordiluong 0:80ac024b84cb 352 stateChanged = true;
jordiluong 0:80ac024b84cb 353 break;
jordiluong 0:80ac024b84cb 354 }
jordiluong 4:ea7689bf97e1 355 break;
jordiluong 0:80ac024b84cb 356 }
jordiluong 0:80ac024b84cb 357
jordiluong 0:80ac024b84cb 358 default:
jordiluong 0:80ac024b84cb 359 {
jordiluong 5:0d3e8694726e 360 TurnMotorsOff(); // Turn motors off for safety
jordiluong 4:ea7689bf97e1 361 break;
jordiluong 0:80ac024b84cb 362 }
jordiluong 0:80ac024b84cb 363 }
jordiluong 0:80ac024b84cb 364 }
jordiluong 0:80ac024b84cb 365
jordiluong 7:757e95b4dc46 366 // MAIN FUNCTION ///////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 367 int main()
jordiluong 0:80ac024b84cb 368 {
jordiluong 0:80ac024b84cb 369 // Serial communication
jordiluong 0:80ac024b84cb 370 pc.baud(115200);
jordiluong 0:80ac024b84cb 371
jordiluong 4:ea7689bf97e1 372 pc.printf("START \r\n");
jordiluong 4:ea7689bf97e1 373
jordiluong 12:12b72bd60fd1 374 //bqc.add(&bq1).add(&bq2); // Make BiQuad Chain
jordiluong 7:757e95b4dc46 375
jordiluong 12:12b72bd60fd1 376 sampleTicker.attach(&EmgSample, tickerTs); // Ticker to sample EMG
jordiluong 12:12b72bd60fd1 377 controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID)
jordiluong 12:12b72bd60fd1 378
jordiluong 12:12b72bd60fd1 379 motor1MagnitudePin.period_ms(1);
jordiluong 12:12b72bd60fd1 380 motor2MagnitudePin.period_ms(1);
jordiluong 12:12b72bd60fd1 381 TurnMotorsOff();
jordiluong 4:ea7689bf97e1 382
jordiluong 0:80ac024b84cb 383 while(true)
jordiluong 0:80ac024b84cb 384 {
jordiluong 0:80ac024b84cb 385 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 386 }
jordiluong 0:80ac024b84cb 387 }