Ruben Lucas / Mbed 2 deprecated StateMachinePTR

StateMachine PTR

Dependencies:   MODSERIAL QEI MovingAverage biquadFilter mbed

StateMachinePTR.cpp@5:cbcff21e74a0, 2018-10-30 (annotated)

Committer:
rubenlucas
Date:
Tue Oct 30 14:57:45 2018 +0000
Revision:
5:cbcff21e74a0
Parent:
4:4728763bbb44
Child:
6:d7ae5f8fd460
including IK and EMG calibration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rubenlucas 2:c2ae5044ec82 1 #include "mbed.h"
rubenlucas 2:c2ae5044ec82 2 #include "math.h"
rubenlucas 2:c2ae5044ec82 3 #include "MODSERIAL.h"
rubenlucas 2:c2ae5044ec82 4 #include "QEI.h"
rubenlucas 2:c2ae5044ec82 5 #include "BiQuad.h"
rubenlucas 3:97cac1d5ba8a 6 #include "MovingAverage.h"
rubenlucas 3:97cac1d5ba8a 7 #define NSAMPLE 200
rubenlucas 3:97cac1d5ba8a 8
rubenlucas 2:c2ae5044ec82 9 //states
rubenlucas 3:97cac1d5ba8a 10 enum States {Waiting, Homing, Emergency, EMGCal, MotorCal, Operation, Demo};
rubenlucas 2:c2ae5044ec82 11
rubenlucas 2:c2ae5044ec82 12 // Global variables
rubenlucas 2:c2ae5044ec82 13 States CurrentState;
rubenlucas 2:c2ae5044ec82 14 Ticker TickerLoop;
rubenlucas 2:c2ae5044ec82 15 Timer TimerLoop;
rubenlucas 2:c2ae5044ec82 16
rubenlucas 2:c2ae5044ec82 17 //Communication
rubenlucas 2:c2ae5044ec82 18 MODSERIAL pc(USBTX,USBRX);
rubenlucas 3:97cac1d5ba8a 19 QEI Encoder1(D10,D11,NC,32); // Encoder motor q1 (arm)
rubenlucas 3:97cac1d5ba8a 20 QEI Encoder2(D12,D13,NC,32); // Encoder motor q2 (end-effector)
rubenlucas 2:c2ae5044ec82 21
rubenlucas 3:97cac1d5ba8a 22 //EMG settings
rubenlucas 3:97cac1d5ba8a 23 AnalogIn emg0(A0); //Biceps Left
rubenlucas 3:97cac1d5ba8a 24 AnalogIn emg1(A1); //Biceps Right
rubenlucas 3:97cac1d5ba8a 25 MovingAverage <double>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
rubenlucas 3:97cac1d5ba8a 26 MovingAverage <double>Movag_RB(NSAMPLE,0.0);
rubenlucas 3:97cac1d5ba8a 27
rubenlucas 3:97cac1d5ba8a 28 // filters
rubenlucas 3:97cac1d5ba8a 29 //Make Biquad filters Left(a0, a1, a2, b1, b2)
rubenlucas 3:97cac1d5ba8a 30 BiQuadChain bqc1; //chain voor High Pass en Notch
rubenlucas 3:97cac1d5ba8a 31 BiQuad bq1(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
rubenlucas 3:97cac1d5ba8a 32 BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
rubenlucas 3:97cac1d5ba8a 33 //Make Biquad filters Right
rubenlucas 3:97cac1d5ba8a 34 BiQuadChain bqc2; //chain voor High Pass en Notch
rubenlucas 3:97cac1d5ba8a 35 BiQuad bq3(0.39131200825291007, -0.7826240165058201, 0.39131200825291007, -0.36950493743858204, 0.1957430955730582); //High Pass Filter
rubenlucas 3:97cac1d5ba8a 36 BiQuad bq4(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
rubenlucas 3:97cac1d5ba8a 37
rubenlucas 3:97cac1d5ba8a 38
rubenlucas 3:97cac1d5ba8a 39 //Global pin variables Motors 1
rubenlucas 3:97cac1d5ba8a 40 PwmOut PwmPin1(D5);
rubenlucas 3:97cac1d5ba8a 41 DigitalOut DirectionPin1(D4);
rubenlucas 3:97cac1d5ba8a 42
rubenlucas 3:97cac1d5ba8a 43 //Global pin variables Motors 2
rubenlucas 3:97cac1d5ba8a 44 PwmOut PwmPin2(D6);
rubenlucas 3:97cac1d5ba8a 45 DigitalOut DirectionPin2(D7);
rubenlucas 2:c2ae5044ec82 46
rubenlucas 2:c2ae5044ec82 47 //Output LED
rubenlucas 2:c2ae5044ec82 48 DigitalOut LedRed (LED_RED);
rubenlucas 2:c2ae5044ec82 49 DigitalOut LedGreen (LED_GREEN);
rubenlucas 2:c2ae5044ec82 50 DigitalOut LedBlue (LED_BLUE);
rubenlucas 2:c2ae5044ec82 51
rubenlucas 3:97cac1d5ba8a 52 // Buttons
rubenlucas 3:97cac1d5ba8a 53 DigitalIn ButtonHoming(SW2); // Button to go to Homing state
rubenlucas 3:97cac1d5ba8a 54 DigitalIn BUTSW3(SW3);
rubenlucas 3:97cac1d5ba8a 55 DigitalIn BUT1(D8);
rubenlucas 3:97cac1d5ba8a 56 DigitalIn BUT2(D9);
rubenlucas 2:c2ae5044ec82 57
rubenlucas 3:97cac1d5ba8a 58 //Constant variables
rubenlucas 3:97cac1d5ba8a 59 const float L0 = 0.1; // Distance between origin frame and joint 1 [m[
rubenlucas 3:97cac1d5ba8a 60 const float L1 = 0.326; // Length link 1 (shaft to shaft) [m]
rubenlucas 3:97cac1d5ba8a 61 const float L2 = 0.209; // Length link 2 (end-effector length) [m]
rubenlucas 4:4728763bbb44 62 const double Ts = 0.002; //Sampling time 500 Hz
rubenlucas 3:97cac1d5ba8a 63
rubenlucas 3:97cac1d5ba8a 64
rubenlucas 3:97cac1d5ba8a 65 // Volatile variables
rubenlucas 3:97cac1d5ba8a 66 //EMG
rubenlucas 3:97cac1d5ba8a 67 volatile bool EMGBoolLeft; // Boolean EMG 1 (left)
rubenlucas 3:97cac1d5ba8a 68 volatile bool EMGBoolRight; // Boolean EMG 2 (right)
rubenlucas 3:97cac1d5ba8a 69 volatile double LeftBicepsOut; // Final value left of processed EMG
rubenlucas 3:97cac1d5ba8a 70 volatile double RightBicepsOut; // Final value right of processed EMG
rubenlucas 3:97cac1d5ba8a 71 volatile double ThresholdLeft = 1; //Threshold to compare with value (start off with max value = 1)
rubenlucas 3:97cac1d5ba8a 72 volatile double ThresholdRight = 1; //Threshold to compare with value (start off with max value = 1)
rubenlucas 3:97cac1d5ba8a 73
rubenlucas 3:97cac1d5ba8a 74
rubenlucas 3:97cac1d5ba8a 75 //Motors
rubenlucas 3:97cac1d5ba8a 76 volatile float q1; // Current angle of motor 1 (arm)
rubenlucas 3:97cac1d5ba8a 77 volatile float q2; // Current angle of motor 2 (end-effector)
rubenlucas 3:97cac1d5ba8a 78 volatile float MotorValue1; // Inputvalue to set motor 1
rubenlucas 3:97cac1d5ba8a 79 volatile float MotorValue2; // Inputvalue to set motor 2
rubenlucas 3:97cac1d5ba8a 80
rubenlucas 3:97cac1d5ba8a 81 //Inverse kinematics
rubenlucas 3:97cac1d5ba8a 82 volatile float VdesX; // Desired velocity in x direction
rubenlucas 3:97cac1d5ba8a 83 volatile float VdesY; // Desired velocity in y direction
rubenlucas 3:97cac1d5ba8a 84 volatile float Error1; // Difference in reference angle and current angle motor 1
rubenlucas 3:97cac1d5ba8a 85 volatile float Error2; // Difference in reference angle and current angle motor 2
rubenlucas 3:97cac1d5ba8a 86
rubenlucas 2:c2ae5044ec82 87
rubenlucas 2:c2ae5044ec82 88
rubenlucas 3:97cac1d5ba8a 89 //------------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 90 //------------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 91
rubenlucas 3:97cac1d5ba8a 92 // Function for processing EMG
rubenlucas 3:97cac1d5ba8a 93 void ProcessingEMG()
rubenlucas 3:97cac1d5ba8a 94 {
rubenlucas 3:97cac1d5ba8a 95 //Filter Left Biceps
rubenlucas 3:97cac1d5ba8a 96 double LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch
rubenlucas 3:97cac1d5ba8a 97 double LB_Rectify = fabs(LB_Filter_1); //Rectify Signal
rubenlucas 3:97cac1d5ba8a 98 Movag_LB.Insert(LB_Rectify); //Moving Average
rubenlucas 3:97cac1d5ba8a 99 LeftBicepsOut = Movag_LB.GetAverage(); //Get final value
rubenlucas 3:97cac1d5ba8a 100
rubenlucas 3:97cac1d5ba8a 101 //Filter Right Biceps
rubenlucas 3:97cac1d5ba8a 102 double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
rubenlucas 3:97cac1d5ba8a 103 double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
rubenlucas 3:97cac1d5ba8a 104 Movag_RB.Insert(RB_Rectify); //Moving Average
rubenlucas 3:97cac1d5ba8a 105 RightBicepsOut = Movag_RB.GetAverage(); //Get final value
rubenlucas 3:97cac1d5ba8a 106
rubenlucas 3:97cac1d5ba8a 107 if (LeftBicepsOut > ThresholdLeft)
rubenlucas 3:97cac1d5ba8a 108 {
rubenlucas 3:97cac1d5ba8a 109 EMGBoolLeft = true;
rubenlucas 3:97cac1d5ba8a 110 }
rubenlucas 3:97cac1d5ba8a 111 else
rubenlucas 3:97cac1d5ba8a 112 {
rubenlucas 3:97cac1d5ba8a 113 EMGBoolLeft = false;
rubenlucas 3:97cac1d5ba8a 114 }
rubenlucas 3:97cac1d5ba8a 115 if (RightBicepsOut > ThresholdRight)
rubenlucas 3:97cac1d5ba8a 116 {
rubenlucas 3:97cac1d5ba8a 117 EMGBoolRight = true;
rubenlucas 3:97cac1d5ba8a 118 }
rubenlucas 3:97cac1d5ba8a 119 else
rubenlucas 3:97cac1d5ba8a 120 {
rubenlucas 3:97cac1d5ba8a 121 EMGBoolRight = false;
rubenlucas 3:97cac1d5ba8a 122 }
rubenlucas 3:97cac1d5ba8a 123
rubenlucas 3:97cac1d5ba8a 124 }
rubenlucas 3:97cac1d5ba8a 125
rubenlucas 3:97cac1d5ba8a 126 void MeasureAll()
rubenlucas 3:97cac1d5ba8a 127 {
rubenlucas 3:97cac1d5ba8a 128 //Processing and reading EMG
rubenlucas 3:97cac1d5ba8a 129 ProcessingEMG();
rubenlucas 3:97cac1d5ba8a 130
rubenlucas 3:97cac1d5ba8a 131 //Measure current motor angles
rubenlucas 3:97cac1d5ba8a 132 q1 = Encoder1.getPulses()/8400*(2*6.28318530718); //angle motor 1 (Times 2 to compensate for endcoding X4 --> X2)
rubenlucas 3:97cac1d5ba8a 133 q2 = Encoder2.getPulses()/8400*(2*6.28318530718); // angle motor 2 (Times 2 to compensate for endcoding X4 --> X2)
rubenlucas 3:97cac1d5ba8a 134
rubenlucas 3:97cac1d5ba8a 135 }
rubenlucas 3:97cac1d5ba8a 136
rubenlucas 3:97cac1d5ba8a 137 //State machine
rubenlucas 3:97cac1d5ba8a 138 void StateMachine()
rubenlucas 2:c2ae5044ec82 139 {
rubenlucas 2:c2ae5044ec82 140 switch (CurrentState)
rubenlucas 2:c2ae5044ec82 141 {
rubenlucas 2:c2ae5044ec82 142 case Waiting:
rubenlucas 2:c2ae5044ec82 143 LedRed = 0;
rubenlucas 2:c2ae5044ec82 144 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 145 LedBlue = 1; //Yellow
rubenlucas 5:cbcff21e74a0 146
rubenlucas 5:cbcff21e74a0 147 if (BUT2 == false)
rubenlucas 5:cbcff21e74a0 148 {
rubenlucas 5:cbcff21e74a0 149 CurrentState = EMGCal;
rubenlucas 5:cbcff21e74a0 150 }
rubenlucas 5:cbcff21e74a0 151
rubenlucas 2:c2ae5044ec82 152 break;
rubenlucas 2:c2ae5044ec82 153
rubenlucas 2:c2ae5044ec82 154 case Homing:
rubenlucas 2:c2ae5044ec82 155 LedRed = 0;
rubenlucas 2:c2ae5044ec82 156 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 157 LedBlue = 0; //White
rubenlucas 2:c2ae5044ec82 158 break;
rubenlucas 2:c2ae5044ec82 159
rubenlucas 2:c2ae5044ec82 160 case Emergency:
rubenlucas 2:c2ae5044ec82 161 LedRed = 0;
rubenlucas 2:c2ae5044ec82 162 LedGreen = 1;
rubenlucas 2:c2ae5044ec82 163 LedBlue = 1; //Red
rubenlucas 2:c2ae5044ec82 164 break;
rubenlucas 2:c2ae5044ec82 165
rubenlucas 2:c2ae5044ec82 166 case EMGCal:
rubenlucas 2:c2ae5044ec82 167 LedRed = 0;
rubenlucas 2:c2ae5044ec82 168 LedGreen = 1;
rubenlucas 2:c2ae5044ec82 169 LedBlue = 0; //Pink
rubenlucas 4:4728763bbb44 170 static double MaxLeft = 0;
rubenlucas 4:4728763bbb44 171 static double MaxRight = 0;
rubenlucas 4:4728763bbb44 172
rubenlucas 4:4728763bbb44 173 if(LeftBicepsOut > MaxLeft)
rubenlucas 4:4728763bbb44 174 {
rubenlucas 4:4728763bbb44 175 MaxLeft = LeftBicepsOut;
rubenlucas 4:4728763bbb44 176 }
rubenlucas 4:4728763bbb44 177
rubenlucas 4:4728763bbb44 178 if(RightBicepsOut > MaxRight)
rubenlucas 4:4728763bbb44 179 {
rubenlucas 4:4728763bbb44 180 MaxRight = RightBicepsOut;
rubenlucas 4:4728763bbb44 181 }
rubenlucas 4:4728763bbb44 182
rubenlucas 4:4728763bbb44 183 if (BUT1 == false)
rubenlucas 4:4728763bbb44 184 {
rubenlucas 4:4728763bbb44 185 ThresholdLeft = abs(0.15000*MaxLeft);
rubenlucas 4:4728763bbb44 186 ThresholdRight = abs(0.15000*MaxRight);
rubenlucas 4:4728763bbb44 187 TimerLoop.reset();
rubenlucas 4:4728763bbb44 188 }
rubenlucas 4:4728763bbb44 189 if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 2))
rubenlucas 4:4728763bbb44 190 {
rubenlucas 4:4728763bbb44 191 CurrentState = MotorCal;
rubenlucas 4:4728763bbb44 192 TimerLoop.reset();
rubenlucas 4:4728763bbb44 193 }
rubenlucas 2:c2ae5044ec82 194 break;
rubenlucas 2:c2ae5044ec82 195
rubenlucas 2:c2ae5044ec82 196 case MotorCal:
rubenlucas 2:c2ae5044ec82 197 LedRed = 1;
rubenlucas 2:c2ae5044ec82 198 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 199 LedBlue = 0; //Turqoise
rubenlucas 2:c2ae5044ec82 200 break;
rubenlucas 2:c2ae5044ec82 201
rubenlucas 2:c2ae5044ec82 202 case Operation:
rubenlucas 2:c2ae5044ec82 203 LedRed = 1;
rubenlucas 2:c2ae5044ec82 204 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 205 LedBlue = 1; //Green
rubenlucas 2:c2ae5044ec82 206 break;
rubenlucas 2:c2ae5044ec82 207
rubenlucas 2:c2ae5044ec82 208 case Demo:
rubenlucas 2:c2ae5044ec82 209 LedRed = 1;
rubenlucas 2:c2ae5044ec82 210 LedGreen = 1;
rubenlucas 2:c2ae5044ec82 211 LedBlue = 0; //Blue
rubenlucas 2:c2ae5044ec82 212 break;
rubenlucas 2:c2ae5044ec82 213
rubenlucas 2:c2ae5044ec82 214
rubenlucas 2:c2ae5044ec82 215 }
rubenlucas 2:c2ae5044ec82 216 }
rubenlucas 2:c2ae5044ec82 217
rubenlucas 3:97cac1d5ba8a 218 //------------------------------------------------------------------------------
rubenlucas 4:4728763bbb44 219 // Function to set desired cartesian velocities of end-effector
rubenlucas 4:4728763bbb44 220 void Vdesired()
rubenlucas 4:4728763bbb44 221 {
rubenlucas 4:4728763bbb44 222 double Vconst = 0.1; // m/s (10 cm per second)
rubenlucas 4:4728763bbb44 223 VdesX = EMGBoolLeft*Vconst; // Left biceps is X-direction
rubenlucas 4:4728763bbb44 224 VdesY = EMGBoolRight*Vconst; // Right biceps is Y-direction
rubenlucas 4:4728763bbb44 225 }
rubenlucas 4:4728763bbb44 226
rubenlucas 3:97cac1d5ba8a 227 // Inverse Kinematics
rubenlucas 4:4728763bbb44 228 void InverseKinematics()
rubenlucas 2:c2ae5044ec82 229 {
rubenlucas 4:4728763bbb44 230 // matrix inverse Jacobian
rubenlucas 4:4728763bbb44 231 double InvJac11 = (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
rubenlucas 4:4728763bbb44 232 double InvJac12 = -(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
rubenlucas 4:4728763bbb44 233 double InvJac21 = -(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
rubenlucas 4:4728763bbb44 234 double InvJac22 = (L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))/((L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) - L1*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1)) - (L2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + cos(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) - L0*cos(q1) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*sin(q2)*(L0 + L1))*(L2*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q1)*(L0 + L1 - cos(q2)*(L0 + L1)) + L0*sin(q1) + L1*sin(q1) - (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) + cos(q1)*sin(q2)*(L0 + L1)));
rubenlucas 4:4728763bbb44 235
rubenlucas 4:4728763bbb44 236 //Gear Ratio's
rubenlucas 4:4728763bbb44 237 double RatioGears = 134.0/30.0; //Gear Ratio for motor 1
rubenlucas 4:4728763bbb44 238 double RatioPulleys = 87.4/27.5; // Gear Ratio for motor 2
rubenlucas 4:4728763bbb44 239
rubenlucas 4:4728763bbb44 240 double q1DotRef = (InvJac11*VdesX + InvJac12*VdesY)*RatioGears; //reference angular velocity motor 1
rubenlucas 4:4728763bbb44 241 double q2DotRef = (InvJac21*VdesX + InvJac22*VdesY)*RatioPulleys; //reference angular velocity motor 2
rubenlucas 4:4728763bbb44 242
rubenlucas 4:4728763bbb44 243 Error1 = q1DotRef*Ts; // difference between qReference and current q1
rubenlucas 4:4728763bbb44 244 Error2 = q2DotRef*Ts; // difference between qReference and current q2
rubenlucas 2:c2ae5044ec82 245 }
rubenlucas 2:c2ae5044ec82 246
rubenlucas 2:c2ae5044ec82 247
rubenlucas 3:97cac1d5ba8a 248 //------------------------------------------------------------------------------
rubenlucas 3:97cac1d5ba8a 249 // controller motor 1
rubenlucas 3:97cac1d5ba8a 250 void PID_Controller1()
rubenlucas 3:97cac1d5ba8a 251 {
rubenlucas 4:4728763bbb44 252 double Kp = 19.8; // proportional gain
rubenlucas 4:4728763bbb44 253 double Ki = 40.9;//integral gain
rubenlucas 4:4728763bbb44 254 double Kd = 3; //derivative gain
rubenlucas 3:97cac1d5ba8a 255 static double ErrorIntegral = 0;
rubenlucas 3:97cac1d5ba8a 256 static double ErrorPrevious = Error1;
rubenlucas 3:97cac1d5ba8a 257 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
rubenlucas 3:97cac1d5ba8a 258
rubenlucas 3:97cac1d5ba8a 259 //Proportional part:
rubenlucas 3:97cac1d5ba8a 260 double u_k = Kp * Error1;
rubenlucas 3:97cac1d5ba8a 261
rubenlucas 3:97cac1d5ba8a 262 //Integral part:
rubenlucas 3:97cac1d5ba8a 263 ErrorIntegral = ErrorIntegral + Error1*Ts;
rubenlucas 3:97cac1d5ba8a 264 double u_i = Ki * ErrorIntegral;
rubenlucas 3:97cac1d5ba8a 265
rubenlucas 3:97cac1d5ba8a 266 //Derivative part:
rubenlucas 3:97cac1d5ba8a 267 double ErrorDerivative = (Error1 - ErrorPrevious)/Ts;
rubenlucas 3:97cac1d5ba8a 268 double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
rubenlucas 3:97cac1d5ba8a 269 double u_d = Kd * FilteredErrorDerivative;
rubenlucas 3:97cac1d5ba8a 270 ErrorPrevious = Error1;
rubenlucas 3:97cac1d5ba8a 271
rubenlucas 3:97cac1d5ba8a 272 MotorValue1 = u_k + u_i + u_d; //This will become the MotorValue to set motor 1
rubenlucas 3:97cac1d5ba8a 273 }
rubenlucas 3:97cac1d5ba8a 274
rubenlucas 3:97cac1d5ba8a 275 // controller motor 2
rubenlucas 3:97cac1d5ba8a 276 void PID_Controller2()
rubenlucas 3:97cac1d5ba8a 277 {
rubenlucas 4:4728763bbb44 278 double Kp = 11.1; // proportional gain
rubenlucas 4:4728763bbb44 279 double Ki = 22.81;//integral gain
rubenlucas 4:4728763bbb44 280 double Kd = 1.7; //derivative gain
rubenlucas 2:c2ae5044ec82 281 static double ErrorIntegral = 0;
rubenlucas 3:97cac1d5ba8a 282 static double ErrorPrevious = Error2;
rubenlucas 2:c2ae5044ec82 283 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
rubenlucas 2:c2ae5044ec82 284
rubenlucas 2:c2ae5044ec82 285 //Proportional part:
rubenlucas 3:97cac1d5ba8a 286 double u_k = Kp * Error2;
rubenlucas 2:c2ae5044ec82 287
rubenlucas 2:c2ae5044ec82 288 //Integral part:
rubenlucas 3:97cac1d5ba8a 289 ErrorIntegral = ErrorIntegral + Error2*Ts;
rubenlucas 2:c2ae5044ec82 290 double u_i = Ki * ErrorIntegral;
rubenlucas 2:c2ae5044ec82 291
rubenlucas 2:c2ae5044ec82 292 //Derivative part:
rubenlucas 3:97cac1d5ba8a 293 double ErrorDerivative = (Error2 - ErrorPrevious)/Ts;
rubenlucas 2:c2ae5044ec82 294 double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
rubenlucas 2:c2ae5044ec82 295 double u_d = Kd * FilteredErrorDerivative;
rubenlucas 3:97cac1d5ba8a 296 ErrorPrevious = Error2;
rubenlucas 2:c2ae5044ec82 297
rubenlucas 3:97cac1d5ba8a 298 MotorValue2 = u_k + u_i + u_d; //This will become the MotorValue to set motor 2
rubenlucas 3:97cac1d5ba8a 299 }
rubenlucas 3:97cac1d5ba8a 300
rubenlucas 3:97cac1d5ba8a 301 // Main function for motorcontrol
rubenlucas 3:97cac1d5ba8a 302 void MotorControllers()
rubenlucas 3:97cac1d5ba8a 303 {
rubenlucas 3:97cac1d5ba8a 304 PID_Controller1();
rubenlucas 3:97cac1d5ba8a 305 PID_Controller2();
rubenlucas 3:97cac1d5ba8a 306
rubenlucas 2:c2ae5044ec82 307 }
rubenlucas 3:97cac1d5ba8a 308 //------------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 309
rubenlucas 2:c2ae5044ec82 310 //Ticker function set motorvalues
rubenlucas 3:97cac1d5ba8a 311 void SetMotors()
rubenlucas 2:c2ae5044ec82 312 {
rubenlucas 3:97cac1d5ba8a 313 // Motor 1
rubenlucas 3:97cac1d5ba8a 314 if (MotorValue1 >=0) // set direction
rubenlucas 2:c2ae5044ec82 315 {
rubenlucas 3:97cac1d5ba8a 316 DirectionPin1 = 1;
rubenlucas 2:c2ae5044ec82 317 }
rubenlucas 2:c2ae5044ec82 318 else
rubenlucas 2:c2ae5044ec82 319 {
rubenlucas 3:97cac1d5ba8a 320 DirectionPin1 = 0;
rubenlucas 2:c2ae5044ec82 321 }
rubenlucas 2:c2ae5044ec82 322
rubenlucas 3:97cac1d5ba8a 323 if (fabs(MotorValue1)>1) // set duty cycle
rubenlucas 3:97cac1d5ba8a 324 {
rubenlucas 3:97cac1d5ba8a 325 PwmPin1 = 1;
rubenlucas 3:97cac1d5ba8a 326 }
rubenlucas 3:97cac1d5ba8a 327 else
rubenlucas 2:c2ae5044ec82 328 {
rubenlucas 3:97cac1d5ba8a 329 PwmPin1 = fabs(MotorValue1);
rubenlucas 3:97cac1d5ba8a 330 }
rubenlucas 3:97cac1d5ba8a 331
rubenlucas 3:97cac1d5ba8a 332 // Motor 2
rubenlucas 3:97cac1d5ba8a 333 if (MotorValue2 >=0) // set direction
rubenlucas 3:97cac1d5ba8a 334 {
rubenlucas 3:97cac1d5ba8a 335 DirectionPin2 = 1;
rubenlucas 2:c2ae5044ec82 336 }
rubenlucas 2:c2ae5044ec82 337 else
rubenlucas 2:c2ae5044ec82 338 {
rubenlucas 3:97cac1d5ba8a 339 DirectionPin2 = 0;
rubenlucas 2:c2ae5044ec82 340 }
rubenlucas 3:97cac1d5ba8a 341
rubenlucas 3:97cac1d5ba8a 342 if (fabs(MotorValue2)>1) // set duty cycle
rubenlucas 3:97cac1d5ba8a 343 {
rubenlucas 3:97cac1d5ba8a 344 PwmPin2 = 1;
rubenlucas 3:97cac1d5ba8a 345 }
rubenlucas 3:97cac1d5ba8a 346 else
rubenlucas 3:97cac1d5ba8a 347 {
rubenlucas 3:97cac1d5ba8a 348 PwmPin2 = fabs(MotorValue2);
rubenlucas 3:97cac1d5ba8a 349 }
rubenlucas 3:97cac1d5ba8a 350
rubenlucas 2:c2ae5044ec82 351 }
rubenlucas 2:c2ae5044ec82 352
rubenlucas 3:97cac1d5ba8a 353 void SetMotorsOff()
rubenlucas 2:c2ae5044ec82 354 {
rubenlucas 3:97cac1d5ba8a 355 // Turn motors off
rubenlucas 3:97cac1d5ba8a 356 PwmPin1 = 0;
rubenlucas 3:97cac1d5ba8a 357 PwmPin2 = 0;
rubenlucas 2:c2ae5044ec82 358 }
rubenlucas 2:c2ae5044ec82 359
rubenlucas 2:c2ae5044ec82 360 //-----------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 361
rubenlucas 2:c2ae5044ec82 362
rubenlucas 2:c2ae5044ec82 363 // Execution function
rubenlucas 2:c2ae5044ec82 364 void LoopFunction()
rubenlucas 2:c2ae5044ec82 365 {
rubenlucas 2:c2ae5044ec82 366 // buttons
rubenlucas 3:97cac1d5ba8a 367 // if (Button1.read() == false) // if pressed
rubenlucas 3:97cac1d5ba8a 368 // {CurrentState = Operation; TimerLoop.reset();}
rubenlucas 2:c2ae5044ec82 369
rubenlucas 3:97cac1d5ba8a 370 // if (Button2.read() == false) // if pressed
rubenlucas 3:97cac1d5ba8a 371 // {CurrentState = Demo; TimerLoop.reset();}
rubenlucas 2:c2ae5044ec82 372
rubenlucas 3:97cac1d5ba8a 373 // if (ButtonHome.read() == false) // if pressed
rubenlucas 3:97cac1d5ba8a 374 // {CurrentState = Homing; TimerLoop.reset();}
rubenlucas 2:c2ae5044ec82 375 //functions
rubenlucas 3:97cac1d5ba8a 376 // StateMachine(); //determine states
rubenlucas 3:97cac1d5ba8a 377 // if (CurrentState >= 4)
rubenlucas 3:97cac1d5ba8a 378 // {MeasureAndControl(); PrintToScreen();}
rubenlucas 3:97cac1d5ba8a 379 // else
rubenlucas 3:97cac1d5ba8a 380 // {SetMotorOff();}
rubenlucas 2:c2ae5044ec82 381 }
rubenlucas 2:c2ae5044ec82 382
rubenlucas 2:c2ae5044ec82 383 int main()
rubenlucas 2:c2ae5044ec82 384 {
rubenlucas 2:c2ae5044ec82 385 pc.baud(115200);
rubenlucas 2:c2ae5044ec82 386 pc.printf("Hi I'm PTR, you can call me Peter!\r\n");
rubenlucas 3:97cac1d5ba8a 387 TimerLoop.start(); // start Timer
rubenlucas 2:c2ae5044ec82 388 CurrentState = Waiting; // set initial state as Waiting
rubenlucas 3:97cac1d5ba8a 389 bqc1.add( &bq1 ).add( &bq2 ); //make BiQuadChain EMG left
rubenlucas 3:97cac1d5ba8a 390 bqc2.add( &bq3 ).add( &bq4 ); //make BiQuadChain EMG right
rubenlucas 2:c2ae5044ec82 391 TickerLoop.attach(&LoopFunction, 0.002f); //ticker for function calls 500 Hz
rubenlucas 2:c2ae5044ec82 392
rubenlucas 2:c2ae5044ec82 393 while (true)
rubenlucas 2:c2ae5044ec82 394 {
rubenlucas 3:97cac1d5ba8a 395
rubenlucas 2:c2ae5044ec82 396 }
rubenlucas 2:c2ae5044ec82 397
rubenlucas 2:c2ae5044ec82 398 }