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@12:12b72bd60fd1, 2017-11-01 (annotated)

Committer:
jordiluong
Date:
Wed Nov 01 06:46:29 2017 +0000
Revision:
12:12b72bd60fd1
Parent:
11:5c06c97c3673
Child:
13:b455e2e8bed2
Changed PWM frequency; Added derivative filter; ...

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