Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
main.cpp@13:ec227b229f3d, 2017-11-01 (annotated)
- Committer:
- jordiluong
- Date:
- Wed Nov 01 11:25:22 2017 +0000
- Revision:
- 13:ec227b229f3d
- Parent:
- 12:12b72bd60fd1
- Child:
- 14:95acac6a07c7
- Child:
- 22:68e3dc374488
Controllers kind of working
Who changed what in which revision?
User | Revision | Line number | New 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 | 13:ec227b229f3d | 8 | |
jordiluong | 5:0d3e8694726e | 9 | const double pi = 3.1415926535897; // Definition of pi |
jordiluong | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 43 | double velocity = 0.05; // X and Y velocity of the end effector when desired |
jordiluong | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 51 | const double motor1_KP = 13; // Position gain [] |
jordiluong | 13:ec227b229f3d | 52 | const double motor1_KI = 7; // Integral gain [] |
jordiluong | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 64 | double motor2Max = 2*pi*25/360; // Maximum rotation of motor 2 [rad] |
jordiluong | 13:ec227b229f3d | 65 | double motor2Min = 2*pi*-28/360; // Minimum rotation of motor 2 [rad] |
jordiluong | 5:0d3e8694726e | 66 | // Controller gains |
jordiluong | 13:ec227b229f3d | 67 | //const double motor2_KP = potmeter1*10; // Position gain [] |
jordiluong | 13:ec227b229f3d | 68 | //const double motor2_KI = potmeter2*20; // Integral gain [] |
jordiluong | 13:ec227b229f3d | 69 | const double motor2_KP = 13; // Position gain [] |
jordiluong | 13:ec227b229f3d | 70 | const double motor2_KI = 5; // Integral gain [] |
jordiluong | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 83 | |
jordiluong | 12:12b72bd60fd1 | 84 | Ticker sampleTicker; |
jordiluong | 12:12b72bd60fd1 | 85 | const double tickerTs = 0.1; // Time step for sampling [s] |
jordiluong | 13:ec227b229f3d | 86 | |
jordiluong | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 98 | |
jordiluong | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | 13:ec227b229f3d | 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 | } |