Ruben Lucas / Mbed 2 deprecated StateMachinePTR

StateMachine PTR

Dependencies:   MODSERIAL QEI MovingAverage biquadFilter mbed

StateMachinePTR.cpp@13:3493825752ac, 2018-11-01 (annotated)

Committer:
rubenlucas
Date:
Thu Nov 01 15:23:34 2018 +0000
Revision:
13:3493825752ac
Parent:
12:d19588a50fc7
Child:
14:14f42a87cbfb
Alles werkend met knopjes

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 13:3493825752ac 10 enum States {Waiting, Homing, HomingDemo, Emergency, EMGCal, MotorCal, Operation, Demo};
rubenlucas 12:d19588a50fc7 11 enum OPStates {OPWait, OPSet, OPState1, OPState2, OPState3, OPState4, OPHoming};
rubenlucas 2:c2ae5044ec82 12
rubenlucas 2:c2ae5044ec82 13 // Global variables
rubenlucas 2:c2ae5044ec82 14 States CurrentState;
rubenlucas 12:d19588a50fc7 15 OPStates CurrentOperationState;
rubenlucas 2:c2ae5044ec82 16 Ticker TickerLoop;
rubenlucas 2:c2ae5044ec82 17 Timer TimerLoop;
rubenlucas 12:d19588a50fc7 18 Timer TimerCheck;
rubenlucas 13:3493825752ac 19 int CountTime=0;
rubenlucas 2:c2ae5044ec82 20
rubenlucas 2:c2ae5044ec82 21 //Communication
rubenlucas 2:c2ae5044ec82 22 MODSERIAL pc(USBTX,USBRX);
rubenlucas 3:97cac1d5ba8a 23 QEI Encoder1(D10,D11,NC,32); // Encoder motor q1 (arm)
rubenlucas 3:97cac1d5ba8a 24 QEI Encoder2(D12,D13,NC,32); // Encoder motor q2 (end-effector)
rubenlucas 2:c2ae5044ec82 25
rubenlucas 3:97cac1d5ba8a 26 //EMG settings
rubenlucas 3:97cac1d5ba8a 27 AnalogIn emg0(A0); //Biceps Left
rubenlucas 3:97cac1d5ba8a 28 AnalogIn emg1(A1); //Biceps Right
rubenlucas 12:d19588a50fc7 29 MovingAverage <float>Movag_LB(NSAMPLE,0.0); //Make Moving Average, Define NSAMPLE above
rubenlucas 12:d19588a50fc7 30 MovingAverage <float>Movag_RB(NSAMPLE,0.0);
rubenlucas 8:428ff7b7715d 31
rubenlucas 8:428ff7b7715d 32 // filters
rubenlucas 8:428ff7b7715d 33 //Make Biquad filters Left(a0, a1, a2, b1, b2)
rubenlucas 3:97cac1d5ba8a 34 BiQuadChain bqc1; //chain voor High Pass en Notch
rubenlucas 13:3493825752ac 35 BiQuad bq1(0.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013);//0.9142135372357942,-1.8284270744715885,0.9142135372357942,-1.821189693211669,0.8356644557315082); //High Pass Filter
rubenlucas 3:97cac1d5ba8a 36 BiQuad bq2(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
rubenlucas 13:3493825752ac 37 BiQuad bq5(0.6370466299626938,1.2740932599253876,0.6370466299626938,1.13958365554699,0.40860286430378506); //Lowpass Filter
rubenlucas 3:97cac1d5ba8a 38 //Make Biquad filters Right
rubenlucas 3:97cac1d5ba8a 39 BiQuadChain bqc2; //chain voor High Pass en Notch
rubenlucas 13:3493825752ac 40 BiQuad bq3(0.9561305540521468,-1.9122611081042935,0.9561305540521468,-1.9103725395337858,0.9141496766748013);//0.9142135372357942,-1.8284270744715885,0.9142135372357942,-1.821189693211669,0.8356644557315082); //High Pass Filter
rubenlucas 3:97cac1d5ba8a 41 BiQuad bq4(9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01); //Notch Filter
rubenlucas 13:3493825752ac 42 BiQuad bq6(0.6370466299626938,1.2740932599253876,0.6370466299626938,1.13958365554699,0.40860286430378506); //Lowpass Filter
rubenlucas 3:97cac1d5ba8a 43
rubenlucas 3:97cac1d5ba8a 44
rubenlucas 3:97cac1d5ba8a 45 //Global pin variables Motors 1
rubenlucas 3:97cac1d5ba8a 46 PwmOut PwmPin1(D5);
rubenlucas 3:97cac1d5ba8a 47 DigitalOut DirectionPin1(D4);
rubenlucas 3:97cac1d5ba8a 48
rubenlucas 3:97cac1d5ba8a 49 //Global pin variables Motors 2
rubenlucas 3:97cac1d5ba8a 50 PwmOut PwmPin2(D6);
rubenlucas 3:97cac1d5ba8a 51 DigitalOut DirectionPin2(D7);
rubenlucas 2:c2ae5044ec82 52
rubenlucas 2:c2ae5044ec82 53 //Output LED
rubenlucas 2:c2ae5044ec82 54 DigitalOut LedRed (LED_RED);
rubenlucas 2:c2ae5044ec82 55 DigitalOut LedGreen (LED_GREEN);
rubenlucas 2:c2ae5044ec82 56 DigitalOut LedBlue (LED_BLUE);
rubenlucas 2:c2ae5044ec82 57
rubenlucas 3:97cac1d5ba8a 58 // Buttons
rubenlucas 6:d7ae5f8fd460 59 DigitalIn BUTSW2(SW2);
rubenlucas 3:97cac1d5ba8a 60 DigitalIn BUTSW3(SW3);
rubenlucas 3:97cac1d5ba8a 61 DigitalIn BUT1(D8);
rubenlucas 3:97cac1d5ba8a 62 DigitalIn BUT2(D9);
rubenlucas 2:c2ae5044ec82 63
rubenlucas 3:97cac1d5ba8a 64 //Constant variables
rubenlucas 3:97cac1d5ba8a 65 const float L0 = 0.1; // Distance between origin frame and joint 1 [m[
rubenlucas 8:428ff7b7715d 66 const float L1 = 0.326; // Length link 1 (shaft to shaft) [m]
rubenlucas 12:d19588a50fc7 67 const float L2 = 0.185; // Length link 2 (end-effector length) [m]
rubenlucas 12:d19588a50fc7 68 const float Ts = 0.002; //Sampling time 500 Hz
rubenlucas 3:97cac1d5ba8a 69
rubenlucas 12:d19588a50fc7 70 // Homing boolean
rubenlucas 12:d19588a50fc7 71 bool MoveHome = false;
rubenlucas 12:d19588a50fc7 72 bool Switch2Demo = false;
rubenlucas 12:d19588a50fc7 73 bool Switch2OP = false;
rubenlucas 8:428ff7b7715d 74
rubenlucas 8:428ff7b7715d 75 // variables
rubenlucas 7:f4f0939f9df3 76 //Motor calibration
rubenlucas 8:428ff7b7715d 77 bool NextMotorFlag = false;
rubenlucas 7:f4f0939f9df3 78
rubenlucas 3:97cac1d5ba8a 79 //EMG
rubenlucas 8:428ff7b7715d 80 bool EMGBoolLeft; // Boolean EMG 1 (left)
rubenlucas 8:428ff7b7715d 81 bool EMGBoolRight; // Boolean EMG 2 (right)
rubenlucas 12:d19588a50fc7 82 float LeftBicepsOut; // Final value left of processed EMG
rubenlucas 12:d19588a50fc7 83 float RightBicepsOut; // Final value right of processed EMG
rubenlucas 12:d19588a50fc7 84 float ThresholdLeft = 1; //Threshold to compare with value (start off with max value = 1)
rubenlucas 12:d19588a50fc7 85 float ThresholdRight = 1; //Threshold to compare with value (start off with max value = 1)
rubenlucas 12:d19588a50fc7 86 float MaxLeft = 0.00001;
rubenlucas 12:d19588a50fc7 87 float MaxRight = 0.00001;
rubenlucas 12:d19588a50fc7 88 float NormLeft;
rubenlucas 12:d19588a50fc7 89 float NormRight;
rubenlucas 3:97cac1d5ba8a 90
rubenlucas 6:d7ae5f8fd460 91 // Reference positions
rubenlucas 8:428ff7b7715d 92 float q1Ref = 0;
rubenlucas 8:428ff7b7715d 93 float q2Ref = 0;
rubenlucas 3:97cac1d5ba8a 94
rubenlucas 3:97cac1d5ba8a 95 //Motors
rubenlucas 8:428ff7b7715d 96 float q1 = 0; // Current angle of motor 1 (arm)
rubenlucas 8:428ff7b7715d 97 float q2 = 0; // Current angle of motor 2 (end-effector)
rubenlucas 10:9c18b5d08c24 98 float qArm = 0; //Current angle of arm
rubenlucas 10:9c18b5d08c24 99 float qEndEff = 0; //Current angle of end-effector
rubenlucas 8:428ff7b7715d 100 float MotorValue1; // Inputvalue to set motor 1
rubenlucas 8:428ff7b7715d 101 float MotorValue2; // Inputvalue to set motor 2
rubenlucas 3:97cac1d5ba8a 102
rubenlucas 3:97cac1d5ba8a 103 //Inverse kinematics
rubenlucas 8:428ff7b7715d 104 float VdesX; // Desired velocity in x direction
rubenlucas 8:428ff7b7715d 105 float VdesY; // Desired velocity in y direction
rubenlucas 8:428ff7b7715d 106 float Error1; // Difference in reference angle and current angle motor 1
rubenlucas 8:428ff7b7715d 107 float Error2; // Difference in reference angle and current angle motor 2
rubenlucas 9:3410f8dd6845 108 int xDir;
rubenlucas 9:3410f8dd6845 109 int yDir;
rubenlucas 13:3493825752ac 110 int Direction;
rubenlucas 11:2ffae0f2110a 111 float RatioGears = 134.0/30.0;
rubenlucas 13:3493825752ac 112 float RatioPulleys = 87.4/27.5;
rubenlucas 6:d7ae5f8fd460 113 //Print to screen
rubenlucas 8:428ff7b7715d 114 int PrintFlag = false;
rubenlucas 8:428ff7b7715d 115 int CounterPrint = 0;
rubenlucas 2:c2ae5044ec82 116
rubenlucas 2:c2ae5044ec82 117
rubenlucas 3:97cac1d5ba8a 118 //------------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 119 //------------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 120
rubenlucas 6:d7ae5f8fd460 121 void PrintToScreen()
rubenlucas 6:d7ae5f8fd460 122 {
rubenlucas 8:428ff7b7715d 123 CounterPrint++;
rubenlucas 8:428ff7b7715d 124 if (CounterPrint == 100) {
rubenlucas 8:428ff7b7715d 125 PrintFlag = true;
rubenlucas 8:428ff7b7715d 126 CounterPrint = 0;
rubenlucas 8:428ff7b7715d 127 }
rubenlucas 6:d7ae5f8fd460 128 }
rubenlucas 6:d7ae5f8fd460 129
rubenlucas 6:d7ae5f8fd460 130 // Function to set motors off
rubenlucas 6:d7ae5f8fd460 131 void SetMotorsOff()
rubenlucas 6:d7ae5f8fd460 132 {
rubenlucas 6:d7ae5f8fd460 133 // Turn motors off
rubenlucas 8:428ff7b7715d 134 PwmPin1 = 0;
rubenlucas 6:d7ae5f8fd460 135 PwmPin2 = 0;
rubenlucas 8:428ff7b7715d 136
rubenlucas 6:d7ae5f8fd460 137 }
rubenlucas 6:d7ae5f8fd460 138
rubenlucas 3:97cac1d5ba8a 139 // Function for processing EMG
rubenlucas 8:428ff7b7715d 140 void ProcessingEMG()
rubenlucas 8:428ff7b7715d 141 {
rubenlucas 3:97cac1d5ba8a 142 //Filter Left Biceps
rubenlucas 12:d19588a50fc7 143 float LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch
rubenlucas 12:d19588a50fc7 144 float LB_Rectify = fabs(LB_Filter_1); //Rectify Signal
rubenlucas 3:97cac1d5ba8a 145 Movag_LB.Insert(LB_Rectify); //Moving Average
rubenlucas 3:97cac1d5ba8a 146 LeftBicepsOut = Movag_LB.GetAverage(); //Get final value
rubenlucas 12:d19588a50fc7 147 NormLeft = LeftBicepsOut/MaxLeft;
rubenlucas 8:428ff7b7715d 148
rubenlucas 3:97cac1d5ba8a 149 //Filter Right Biceps
rubenlucas 12:d19588a50fc7 150 float RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
rubenlucas 12:d19588a50fc7 151 float RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
rubenlucas 8:428ff7b7715d 152 Movag_RB.Insert(RB_Rectify); //Moving Average
rubenlucas 3:97cac1d5ba8a 153 RightBicepsOut = Movag_RB.GetAverage(); //Get final value
rubenlucas 12:d19588a50fc7 154 NormRight = RightBicepsOut/MaxRight;
rubenlucas 8:428ff7b7715d 155
rubenlucas 12:d19588a50fc7 156 if (NormLeft > ThresholdLeft) {
rubenlucas 8:428ff7b7715d 157 EMGBoolLeft = true;
rubenlucas 12:d19588a50fc7 158 } else {
rubenlucas 8:428ff7b7715d 159 EMGBoolLeft = false;
rubenlucas 8:428ff7b7715d 160 }
rubenlucas 12:d19588a50fc7 161 if (NormRight > ThresholdRight) {
rubenlucas 8:428ff7b7715d 162 EMGBoolRight = true;
rubenlucas 12:d19588a50fc7 163 } else {
rubenlucas 8:428ff7b7715d 164 EMGBoolRight = false;
rubenlucas 8:428ff7b7715d 165 }
rubenlucas 8:428ff7b7715d 166
rubenlucas 8:428ff7b7715d 167 }
rubenlucas 3:97cac1d5ba8a 168
rubenlucas 3:97cac1d5ba8a 169 void MeasureAll()
rubenlucas 3:97cac1d5ba8a 170 {
rubenlucas 3:97cac1d5ba8a 171 //Processing and reading EMG
rubenlucas 3:97cac1d5ba8a 172 ProcessingEMG();
rubenlucas 3:97cac1d5ba8a 173
rubenlucas 3:97cac1d5ba8a 174 //Measure current motor angles
rubenlucas 7:f4f0939f9df3 175 float Counts1 = Encoder1.getPulses();
rubenlucas 7:f4f0939f9df3 176 float Counts2 = Encoder2.getPulses();
rubenlucas 7:f4f0939f9df3 177 q1 = Counts1/8400*(2*6.28318530718); //angle motor 1 (Times 2 to compensate for endcoding X4 --> X2)
rubenlucas 7:f4f0939f9df3 178 q2 = Counts2/8400*(2*6.28318530718); // angle motor 2 (Times 2 to compensate for endcoding X4 --> X2)
rubenlucas 12:d19588a50fc7 179
rubenlucas 12:d19588a50fc7 180
rubenlucas 10:9c18b5d08c24 181 qArm = -1*q1/RatioGears; //Adjust for opposite direction due to gears
rubenlucas 12:d19588a50fc7 182 qEndEff = q2/RatioPulleys;
rubenlucas 8:428ff7b7715d 183
rubenlucas 7:f4f0939f9df3 184
rubenlucas 8:428ff7b7715d 185
rubenlucas 3:97cac1d5ba8a 186 }
rubenlucas 3:97cac1d5ba8a 187
rubenlucas 6:d7ae5f8fd460 188 // Function to set desired cartesian velocities of end-effector
rubenlucas 6:d7ae5f8fd460 189 void Vdesired()
rubenlucas 6:d7ae5f8fd460 190 {
rubenlucas 13:3493825752ac 191 if(BUTSW3 == false)
rubenlucas 13:3493825752ac 192 {
rubenlucas 13:3493825752ac 193 Direction = -1;
rubenlucas 13:3493825752ac 194 }
rubenlucas 13:3493825752ac 195 else
rubenlucas 13:3493825752ac 196 {
rubenlucas 13:3493825752ac 197 Direction = 1;
rubenlucas 13:3493825752ac 198 }
rubenlucas 9:3410f8dd6845 199 xDir = 1-BUT1.read();
rubenlucas 9:3410f8dd6845 200 yDir = 1-BUT2.read();
rubenlucas 13:3493825752ac 201 float Vconst = 0.05; // m/s (3 cm per second)
rubenlucas 13:3493825752ac 202 VdesX = Direction*xDir*Vconst; //EMGBoolLeft*Vconst; // Left biceps is X-direction
rubenlucas 13:3493825752ac 203 VdesY = Direction*yDir*Vconst;//-1*EMGBoolRight*Vconst; // Right biceps is minus Y-direction
rubenlucas 6:d7ae5f8fd460 204 }
rubenlucas 6:d7ae5f8fd460 205
rubenlucas 6:d7ae5f8fd460 206 // Inverse Kinematics
rubenlucas 6:d7ae5f8fd460 207 void InverseKinematics()
rubenlucas 6:d7ae5f8fd460 208 {
rubenlucas 6:d7ae5f8fd460 209 // matrix inverse Jacobian
rubenlucas 12:d19588a50fc7 210 float InvJac11 = (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1)));
rubenlucas 12:d19588a50fc7 211 float InvJac12 = -(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1)));
rubenlucas 12:d19588a50fc7 212 float InvJac21 = -(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1)));
rubenlucas 12:d19588a50fc7 213 float InvJac22 = (L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))/((L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) - L1*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1)) - (L2*(cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm)) + cos(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) - L0*cos(qArm) + (cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff))*(L0 + L1) + sin(qArm)*sin(qEndEff)*(L0 + L1))*(L2*(cos(qArm)*cos(qEndEff) - sin(qArm)*sin(qEndEff)) - sin(qArm)*(L0 + L1 - cos(qEndEff)*(L0 + L1)) + L0*sin(qArm) + L1*sin(qArm) - (cos(qArm)*sin(qEndEff) + cos(qEndEff)*sin(qArm))*(L0 + L1) + cos(qArm)*sin(qEndEff)*(L0 + L1)));
rubenlucas 8:428ff7b7715d 214
rubenlucas 10:9c18b5d08c24 215
rubenlucas 8:428ff7b7715d 216
rubenlucas 12:d19588a50fc7 217 float q1DotRef = (InvJac11*VdesX + InvJac12*VdesY); //reference angular velocity motor 1
rubenlucas 12:d19588a50fc7 218 float q2DotRef = (InvJac21*VdesX + InvJac22*VdesY); //reference angular velocity motor 2
rubenlucas 8:428ff7b7715d 219
rubenlucas 9:3410f8dd6845 220 q1Ref += q1DotRef*Ts; // set new reference position motor angle 1
rubenlucas 9:3410f8dd6845 221 q2Ref += q2DotRef*Ts; // set new reference position motor angle 1
rubenlucas 6:d7ae5f8fd460 222 }
rubenlucas 6:d7ae5f8fd460 223
rubenlucas 3:97cac1d5ba8a 224 //State machine
rubenlucas 3:97cac1d5ba8a 225 void StateMachine()
rubenlucas 2:c2ae5044ec82 226 {
rubenlucas 8:428ff7b7715d 227 switch (CurrentState) {
rubenlucas 2:c2ae5044ec82 228 case Waiting:
rubenlucas 6:d7ae5f8fd460 229 SetMotorsOff();
rubenlucas 7:f4f0939f9df3 230 Encoder1.reset();
rubenlucas 7:f4f0939f9df3 231 q1Ref = 0;
rubenlucas 7:f4f0939f9df3 232 q1 = 0;
rubenlucas 7:f4f0939f9df3 233 Error1 = 0;
rubenlucas 7:f4f0939f9df3 234 Encoder2.reset();
rubenlucas 7:f4f0939f9df3 235 q2Ref = 0;
rubenlucas 7:f4f0939f9df3 236 q2 = 0;
rubenlucas 7:f4f0939f9df3 237 Error2 = 0;
rubenlucas 8:428ff7b7715d 238
rubenlucas 2:c2ae5044ec82 239 LedRed = 0;
rubenlucas 2:c2ae5044ec82 240 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 241 LedBlue = 1; //Yellow
rubenlucas 8:428ff7b7715d 242
rubenlucas 8:428ff7b7715d 243 if (BUT2 == false) {
rubenlucas 5:cbcff21e74a0 244 CurrentState = EMGCal;
rubenlucas 6:d7ae5f8fd460 245 TimerLoop.reset();
rubenlucas 5:cbcff21e74a0 246 }
rubenlucas 8:428ff7b7715d 247
rubenlucas 8:428ff7b7715d 248 break;
rubenlucas 8:428ff7b7715d 249
rubenlucas 2:c2ae5044ec82 250 case Homing:
rubenlucas 2:c2ae5044ec82 251 LedRed = 0;
rubenlucas 2:c2ae5044ec82 252 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 253 LedBlue = 0; //White
rubenlucas 12:d19588a50fc7 254
rubenlucas 12:d19588a50fc7 255
rubenlucas 12:d19588a50fc7 256 if (MoveHome == true) {
rubenlucas 12:d19588a50fc7 257 if (q1Ref >= 0) {
rubenlucas 12:d19588a50fc7 258 q1Ref -= 0.0005*(6.380);
rubenlucas 12:d19588a50fc7 259 }
rubenlucas 12:d19588a50fc7 260 if (q1Ref <= 0) {
rubenlucas 12:d19588a50fc7 261 q1Ref += 0.0005*(6.380);
rubenlucas 12:d19588a50fc7 262 }
rubenlucas 12:d19588a50fc7 263 if (q2Ref >= 0) {
rubenlucas 12:d19588a50fc7 264 q2Ref -= 0.0005*(6.380);
rubenlucas 12:d19588a50fc7 265 }
rubenlucas 12:d19588a50fc7 266 if (q2Ref <= 0) {
rubenlucas 12:d19588a50fc7 267 q2Ref += 0.0005*(6.380);
rubenlucas 12:d19588a50fc7 268 }
rubenlucas 12:d19588a50fc7 269
rubenlucas 12:d19588a50fc7 270 if (TimerLoop.read()>=5.0) { //(-0.002*(6.380) <= q1Ref <= 0.002*(6.380)) && (-0.002*(6.380) <= q2Ref <= 0.002*(6.380))
rubenlucas 12:d19588a50fc7 271 MoveHome = false;
rubenlucas 12:d19588a50fc7 272 SetMotorsOff();
rubenlucas 12:d19588a50fc7 273
rubenlucas 12:d19588a50fc7 274 }
rubenlucas 12:d19588a50fc7 275
rubenlucas 12:d19588a50fc7 276 } //End of if(movehome) statement
rubenlucas 12:d19588a50fc7 277
rubenlucas 12:d19588a50fc7 278
rubenlucas 12:d19588a50fc7 279 if (MoveHome == false) {
rubenlucas 12:d19588a50fc7 280 if (BUT2 == false) {
rubenlucas 12:d19588a50fc7 281 Switch2Demo = true;
rubenlucas 12:d19588a50fc7 282 TimerLoop.reset();
rubenlucas 12:d19588a50fc7 283 }
rubenlucas 12:d19588a50fc7 284 if ((Switch2Demo == true) && (TimerLoop.read()>=2.0)) {
rubenlucas 12:d19588a50fc7 285 CurrentState = Demo;
rubenlucas 12:d19588a50fc7 286 Switch2Demo = false;
rubenlucas 12:d19588a50fc7 287 }
rubenlucas 12:d19588a50fc7 288 if (BUT1 == false) {
rubenlucas 12:d19588a50fc7 289 Switch2OP = true;
rubenlucas 12:d19588a50fc7 290 TimerLoop.reset();
rubenlucas 12:d19588a50fc7 291 }
rubenlucas 12:d19588a50fc7 292 if ((Switch2OP == true) && (TimerLoop.read()>=2.0)) {
rubenlucas 12:d19588a50fc7 293 CurrentState = Operation;
rubenlucas 12:d19588a50fc7 294 Switch2OP = false;
rubenlucas 12:d19588a50fc7 295 }
rubenlucas 12:d19588a50fc7 296
rubenlucas 12:d19588a50fc7 297 } //End of else statement
rubenlucas 12:d19588a50fc7 298
rubenlucas 8:428ff7b7715d 299 break;
rubenlucas 8:428ff7b7715d 300
rubenlucas 13:3493825752ac 301 case HomingDemo:
rubenlucas 13:3493825752ac 302 if (MoveHome == true) {
rubenlucas 13:3493825752ac 303 if (q1Ref >= 0) {
rubenlucas 13:3493825752ac 304 q1Ref -= 0.0005*(6.380);
rubenlucas 13:3493825752ac 305 }
rubenlucas 13:3493825752ac 306 if (q1Ref <= 0) {
rubenlucas 13:3493825752ac 307 q1Ref += 0.0005*(6.380);
rubenlucas 13:3493825752ac 308 }
rubenlucas 13:3493825752ac 309 if (q2Ref >= 0) {
rubenlucas 13:3493825752ac 310 q2Ref -= 0.0005*(6.380);
rubenlucas 13:3493825752ac 311 }
rubenlucas 13:3493825752ac 312 if (q2Ref <= 0) {
rubenlucas 13:3493825752ac 313 q2Ref += 0.0005*(6.380);
rubenlucas 13:3493825752ac 314 }
rubenlucas 13:3493825752ac 315
rubenlucas 13:3493825752ac 316 if (TimerLoop.read()>=5.0) { //(-0.002*(6.380) <= q1Ref <= 0.002*(6.380)) && (-0.002*(6.380) <= q2Ref <= 0.002*(6.380))
rubenlucas 13:3493825752ac 317 MoveHome = false;
rubenlucas 13:3493825752ac 318 CurrentState = Homing;
rubenlucas 13:3493825752ac 319 SetMotorsOff();
rubenlucas 13:3493825752ac 320
rubenlucas 13:3493825752ac 321 }
rubenlucas 13:3493825752ac 322 }
rubenlucas 13:3493825752ac 323 break;
rubenlucas 13:3493825752ac 324
rubenlucas 2:c2ae5044ec82 325 case Emergency:
rubenlucas 2:c2ae5044ec82 326 LedRed = 0;
rubenlucas 2:c2ae5044ec82 327 LedGreen = 1;
rubenlucas 2:c2ae5044ec82 328 LedBlue = 1; //Red
rubenlucas 8:428ff7b7715d 329 break;
rubenlucas 8:428ff7b7715d 330
rubenlucas 2:c2ae5044ec82 331 case EMGCal:
rubenlucas 2:c2ae5044ec82 332 LedRed = 0;
rubenlucas 2:c2ae5044ec82 333 LedGreen = 1;
rubenlucas 2:c2ae5044ec82 334 LedBlue = 0; //Pink
rubenlucas 12:d19588a50fc7 335
rubenlucas 8:428ff7b7715d 336
rubenlucas 8:428ff7b7715d 337 if(LeftBicepsOut > MaxLeft) {
rubenlucas 8:428ff7b7715d 338 MaxLeft = LeftBicepsOut;
rubenlucas 13:3493825752ac 339 ThresholdLeft = abs(0.6000);
rubenlucas 8:428ff7b7715d 340 TimerLoop.reset();
rubenlucas 8:428ff7b7715d 341 }
rubenlucas 8:428ff7b7715d 342
rubenlucas 8:428ff7b7715d 343 if(RightBicepsOut > MaxRight) {
rubenlucas 8:428ff7b7715d 344 MaxRight = RightBicepsOut;
rubenlucas 13:3493825752ac 345 ThresholdRight = abs(0.6000);
rubenlucas 4:4728763bbb44 346 TimerLoop.reset();
rubenlucas 4:4728763bbb44 347 }
rubenlucas 8:428ff7b7715d 348
rubenlucas 8:428ff7b7715d 349 if (BUT1 == false) {
rubenlucas 8:428ff7b7715d 350 //ThresholdLeft = abs(0.15000*MaxLeft);
rubenlucas 8:428ff7b7715d 351 //ThresholdRight = abs(0.15000*MaxRight);
rubenlucas 8:428ff7b7715d 352 }
rubenlucas 8:428ff7b7715d 353 if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 5.0)) {
rubenlucas 4:4728763bbb44 354 TimerLoop.reset();
rubenlucas 8:428ff7b7715d 355 CurrentState = MotorCal;
rubenlucas 4:4728763bbb44 356 }
rubenlucas 8:428ff7b7715d 357 break;
rubenlucas 8:428ff7b7715d 358
rubenlucas 2:c2ae5044ec82 359 case MotorCal:
rubenlucas 2:c2ae5044ec82 360 LedRed = 1;
rubenlucas 2:c2ae5044ec82 361 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 362 LedBlue = 0; //Turqoise
rubenlucas 8:428ff7b7715d 363 if (NextMotorFlag == false) {
rubenlucas 8:428ff7b7715d 364 if (BUT1==false) {
rubenlucas 8:428ff7b7715d 365 q1Ref += 0.0005*(6.2830);
rubenlucas 7:f4f0939f9df3 366 }
rubenlucas 8:428ff7b7715d 367 if (BUTSW3 == false) {
rubenlucas 8:428ff7b7715d 368 q1Ref = 0;
rubenlucas 8:428ff7b7715d 369 Encoder1.reset();
rubenlucas 8:428ff7b7715d 370 }
rubenlucas 8:428ff7b7715d 371 if (BUT2 == false) {
rubenlucas 8:428ff7b7715d 372 q1Ref -=0.0005*(6.2830);
rubenlucas 7:f4f0939f9df3 373 }
rubenlucas 8:428ff7b7715d 374 if (BUTSW2 == false) {
rubenlucas 9:3410f8dd6845 375 if (q1Ref>=-0.52*(6.2830)) {
rubenlucas 9:3410f8dd6845 376 q1Ref -=0.0005*(6.2830);
rubenlucas 8:428ff7b7715d 377 } else {
rubenlucas 8:428ff7b7715d 378 TimerLoop.reset();
rubenlucas 8:428ff7b7715d 379 NextMotorFlag = true;
rubenlucas 8:428ff7b7715d 380 }
rubenlucas 8:428ff7b7715d 381
rubenlucas 8:428ff7b7715d 382 } //End of if (BUTSW2 == false)
rubenlucas 8:428ff7b7715d 383 } //End of if (NextMotorFlag == false)
rubenlucas 8:428ff7b7715d 384
rubenlucas 8:428ff7b7715d 385 else if ((NextMotorFlag == true) && (TimerLoop.read()>= 2)) {
rubenlucas 8:428ff7b7715d 386 if (BUT1==false) {
rubenlucas 8:428ff7b7715d 387 q2Ref += 0.0005*(6.2830);
rubenlucas 8:428ff7b7715d 388 }
rubenlucas 8:428ff7b7715d 389 if (BUTSW3 == false) {
rubenlucas 8:428ff7b7715d 390 q2Ref = 0;
rubenlucas 8:428ff7b7715d 391 Encoder2.reset();
rubenlucas 7:f4f0939f9df3 392 }
rubenlucas 8:428ff7b7715d 393 if (BUT2 == false) {
rubenlucas 8:428ff7b7715d 394 q2Ref -= 0.0005*(6.2830);
rubenlucas 7:f4f0939f9df3 395 }
rubenlucas 8:428ff7b7715d 396 if (BUTSW2 == false) {
rubenlucas 9:3410f8dd6845 397 if (q2Ref<=0.733*(6.2830)) {
rubenlucas 9:3410f8dd6845 398 q2Ref +=0.0005*(6.2830);
rubenlucas 8:428ff7b7715d 399 } else {
rubenlucas 8:428ff7b7715d 400 CurrentState = Homing;
rubenlucas 8:428ff7b7715d 401 Encoder1.reset();
rubenlucas 8:428ff7b7715d 402 Encoder2.reset();
rubenlucas 8:428ff7b7715d 403 q1Ref = 0;
rubenlucas 8:428ff7b7715d 404 q2Ref = 0;
rubenlucas 9:3410f8dd6845 405 Error1 = 0;
rubenlucas 9:3410f8dd6845 406 Error2 = 0;
rubenlucas 9:3410f8dd6845 407 q1 = 0;
rubenlucas 9:3410f8dd6845 408 q2 = 0;
rubenlucas 8:428ff7b7715d 409 TimerLoop.reset();
rubenlucas 8:428ff7b7715d 410 }
rubenlucas 8:428ff7b7715d 411 } // end of if (BUTSW2) statement
rubenlucas 8:428ff7b7715d 412 }// end of else if statement
rubenlucas 8:428ff7b7715d 413
rubenlucas 8:428ff7b7715d 414 break;
rubenlucas 8:428ff7b7715d 415
rubenlucas 2:c2ae5044ec82 416 case Operation:
rubenlucas 2:c2ae5044ec82 417 LedRed = 1;
rubenlucas 2:c2ae5044ec82 418 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 419 LedBlue = 1; //Green
rubenlucas 12:d19588a50fc7 420
rubenlucas 12:d19588a50fc7 421 if (BUTSW2 == false) {
rubenlucas 12:d19588a50fc7 422 CurrentState = Homing;
rubenlucas 12:d19588a50fc7 423 CurrentOperationState = OPSet;
rubenlucas 12:d19588a50fc7 424 MoveHome = true;
rubenlucas 12:d19588a50fc7 425 TimerLoop.reset();
rubenlucas 12:d19588a50fc7 426 }
rubenlucas 12:d19588a50fc7 427
rubenlucas 12:d19588a50fc7 428 switch (CurrentOperationState) {
rubenlucas 12:d19588a50fc7 429 case OPWait:
rubenlucas 13:3493825752ac 430 if (EMGBoolLeft == true) { //When Left Biceps are contracted, initiate chain to flip the page
rubenlucas 12:d19588a50fc7 431 CurrentOperationState = OPState1;
rubenlucas 12:d19588a50fc7 432 TimerLoop.reset();
rubenlucas 12:d19588a50fc7 433 }
rubenlucas 12:d19588a50fc7 434 break;
rubenlucas 12:d19588a50fc7 435
rubenlucas 12:d19588a50fc7 436 case OPSet: // Set new homing for Operation Mode and go back to waiting mode
rubenlucas 12:d19588a50fc7 437 if (q2Ref > -0.733*(6.380)) {
rubenlucas 12:d19588a50fc7 438 q2Ref -= 0.0005*(6.380);
rubenlucas 12:d19588a50fc7 439 }
rubenlucas 12:d19588a50fc7 440 if (q2Ref < -0.733*(6.380) && TimerLoop > 2) {
rubenlucas 12:d19588a50fc7 441 CurrentOperationState = OPWait;
rubenlucas 12:d19588a50fc7 442 TimerLoop.reset();
rubenlucas 12:d19588a50fc7 443 }
rubenlucas 12:d19588a50fc7 444 break;
rubenlucas 12:d19588a50fc7 445
rubenlucas 12:d19588a50fc7 446 case OPState1: // First step of chain to flip the page
rubenlucas 12:d19588a50fc7 447 if (q1Ref < 0.48*(6.380)) { //counts/8400 *2 because encoder X2 -> X4
rubenlucas 12:d19588a50fc7 448 q1Ref += 0.0005*(6.380);
rubenlucas 12:d19588a50fc7 449 }
rubenlucas 12:d19588a50fc7 450 if ((q1Ref > 0.48*(6.380)) && (TimerLoop >= 2.0)) {
rubenlucas 12:d19588a50fc7 451 CurrentOperationState = OPState2;
rubenlucas 12:d19588a50fc7 452 TimerLoop.reset();
rubenlucas 12:d19588a50fc7 453 }
rubenlucas 12:d19588a50fc7 454 break;
rubenlucas 12:d19588a50fc7 455
rubenlucas 12:d19588a50fc7 456 case OPState2: //Second step of chain to flip the page
rubenlucas 12:d19588a50fc7 457 if (q2Ref > -1.133*(6.380)) {
rubenlucas 12:d19588a50fc7 458 q2Ref -= 0.0005*(6.380);
rubenlucas 12:d19588a50fc7 459 }
rubenlucas 12:d19588a50fc7 460 if ((q2Ref < -1.133*(6.380)) && (TimerLoop >= 2.0)) {
rubenlucas 12:d19588a50fc7 461 CurrentOperationState = OPState3;
rubenlucas 12:d19588a50fc7 462 TimerLoop.reset();
rubenlucas 12:d19588a50fc7 463 }
rubenlucas 12:d19588a50fc7 464 break;
rubenlucas 12:d19588a50fc7 465
rubenlucas 12:d19588a50fc7 466 case OPState3: //Third step of chain to flip the page
rubenlucas 12:d19588a50fc7 467 if (q1Ref > -0.15*(6.380)) {
rubenlucas 12:d19588a50fc7 468 q1Ref -= 0.0005*(6.380);
rubenlucas 12:d19588a50fc7 469 }
rubenlucas 12:d19588a50fc7 470 if (q2Ref > -1.483*(6.380)) {
rubenlucas 12:d19588a50fc7 471 q2Ref -= 0.0003*(6.380);
rubenlucas 12:d19588a50fc7 472 }
rubenlucas 12:d19588a50fc7 473 if ((q1Ref < -0.15*(6.380)) && (q2Ref < -1.483*(6.380)) && (TimerLoop >= 3.0)) {
rubenlucas 12:d19588a50fc7 474 CurrentOperationState = OPState4;
rubenlucas 12:d19588a50fc7 475 TimerLoop.reset();
rubenlucas 12:d19588a50fc7 476 }
rubenlucas 12:d19588a50fc7 477 break;
rubenlucas 12:d19588a50fc7 478
rubenlucas 12:d19588a50fc7 479 case OPState4: //Fourth step of chain to flip the page
rubenlucas 12:d19588a50fc7 480 if (q2Ref > -2.133*(6.380)) {
rubenlucas 12:d19588a50fc7 481 q2Ref -= 0.005*(6.380);
rubenlucas 12:d19588a50fc7 482 }
rubenlucas 12:d19588a50fc7 483 if ((q2Ref < -2.133*(6.380)) && (TimerLoop > 0.5)) {
rubenlucas 12:d19588a50fc7 484 TimerLoop.reset();
rubenlucas 12:d19588a50fc7 485 CurrentOperationState = OPHoming;
rubenlucas 12:d19588a50fc7 486 }
rubenlucas 12:d19588a50fc7 487 break;
rubenlucas 12:d19588a50fc7 488
rubenlucas 12:d19588a50fc7 489 case OPHoming: //Go back to Home for Operation Mode and back to Waiting
rubenlucas 12:d19588a50fc7 490 if (q1Ref < 0) {
rubenlucas 12:d19588a50fc7 491 q1Ref += 0.003*(6.380);
rubenlucas 12:d19588a50fc7 492 }
rubenlucas 12:d19588a50fc7 493 if (q2Ref < -0.733*(6.380)) {
rubenlucas 12:d19588a50fc7 494 q2Ref += 0.001*(6.380);
rubenlucas 12:d19588a50fc7 495 }
rubenlucas 12:d19588a50fc7 496 if ((q1Ref > 0) && (q2Ref > -0.733*(6.380)) && (TimerLoop > 3)) {
rubenlucas 12:d19588a50fc7 497 CurrentOperationState = OPWait;
rubenlucas 12:d19588a50fc7 498 }
rubenlucas 12:d19588a50fc7 499 break;
rubenlucas 12:d19588a50fc7 500 }
rubenlucas 12:d19588a50fc7 501
rubenlucas 12:d19588a50fc7 502
rubenlucas 8:428ff7b7715d 503 break;
rubenlucas 8:428ff7b7715d 504
rubenlucas 12:d19588a50fc7 505
rubenlucas 2:c2ae5044ec82 506 case Demo:
rubenlucas 2:c2ae5044ec82 507 LedRed = 1;
rubenlucas 2:c2ae5044ec82 508 LedGreen = 1;
rubenlucas 8:428ff7b7715d 509 LedBlue = 0; //Blue
rubenlucas 8:428ff7b7715d 510
rubenlucas 8:428ff7b7715d 511 TimerLoop.reset();
rubenlucas 8:428ff7b7715d 512 Vdesired();
rubenlucas 8:428ff7b7715d 513 InverseKinematics();
rubenlucas 12:d19588a50fc7 514
rubenlucas 12:d19588a50fc7 515 if (BUTSW2 == false) {
rubenlucas 13:3493825752ac 516 CurrentState = HomingDemo;
rubenlucas 12:d19588a50fc7 517 MoveHome = true;
rubenlucas 12:d19588a50fc7 518 TimerLoop.reset();
rubenlucas 12:d19588a50fc7 519 }
rubenlucas 12:d19588a50fc7 520
rubenlucas 8:428ff7b7715d 521
rubenlucas 8:428ff7b7715d 522 break;
rubenlucas 8:428ff7b7715d 523
rubenlucas 2:c2ae5044ec82 524
rubenlucas 7:f4f0939f9df3 525 } //End of switch
rubenlucas 7:f4f0939f9df3 526 } // End of stateMachine function
rubenlucas 2:c2ae5044ec82 527
rubenlucas 3:97cac1d5ba8a 528 //------------------------------------------------------------------------------
rubenlucas 4:4728763bbb44 529
rubenlucas 7:f4f0939f9df3 530 void SetErrors()
rubenlucas 7:f4f0939f9df3 531 {
rubenlucas 13:3493825752ac 532 if ((CurrentState == Demo) || (CurrentState == HomingDemo)) {
rubenlucas 12:d19588a50fc7 533 Error1 = -1*RatioGears*(q1Ref - qArm);
rubenlucas 12:d19588a50fc7 534 Error2 = RatioPulleys*(q2Ref - qEndEff);
rubenlucas 12:d19588a50fc7 535 } else {
rubenlucas 12:d19588a50fc7 536 Error1 = q1Ref - q1;
rubenlucas 12:d19588a50fc7 537 Error2 = q2Ref - q2;
rubenlucas 12:d19588a50fc7 538 }
rubenlucas 7:f4f0939f9df3 539 }
rubenlucas 3:97cac1d5ba8a 540 //------------------------------------------------------------------------------
rubenlucas 3:97cac1d5ba8a 541 // controller motor 1
rubenlucas 12:d19588a50fc7 542 void PID_Controller1(float Err1)
rubenlucas 8:428ff7b7715d 543 {
rubenlucas 12:d19588a50fc7 544 float Kp = 19.8; // proportional gain
rubenlucas 12:d19588a50fc7 545 float Ki = 3.98;//integral gain
rubenlucas 12:d19588a50fc7 546 float Kd = 1.96; //derivative gain
rubenlucas 12:d19588a50fc7 547 static float ErrorIntegral = 0;
rubenlucas 12:d19588a50fc7 548 static float ErrorPrevious = Err1;
rubenlucas 8:428ff7b7715d 549 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
rubenlucas 8:428ff7b7715d 550
rubenlucas 8:428ff7b7715d 551 //Proportional part:
rubenlucas 12:d19588a50fc7 552 float u_k = Kp * Err1;
rubenlucas 8:428ff7b7715d 553
rubenlucas 8:428ff7b7715d 554 //Integral part:
rubenlucas 8:428ff7b7715d 555 ErrorIntegral = ErrorIntegral + Err1*Ts;
rubenlucas 12:d19588a50fc7 556 float u_i = Ki * ErrorIntegral;
rubenlucas 8:428ff7b7715d 557
rubenlucas 8:428ff7b7715d 558 //Derivative part:
rubenlucas 12:d19588a50fc7 559 float ErrorDerivative = (Err1 - ErrorPrevious)/Ts;
rubenlucas 12:d19588a50fc7 560 float FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
rubenlucas 12:d19588a50fc7 561 float u_d = Kd * FilteredErrorDerivative;
rubenlucas 8:428ff7b7715d 562 ErrorPrevious = Err1;
rubenlucas 8:428ff7b7715d 563
rubenlucas 8:428ff7b7715d 564 MotorValue1 = u_k + u_i + u_d; //This will become the MotorValue to set motor 1
rubenlucas 8:428ff7b7715d 565 }
rubenlucas 8:428ff7b7715d 566
rubenlucas 3:97cac1d5ba8a 567 // controller motor 2
rubenlucas 12:d19588a50fc7 568 void PID_Controller2(float Err2)
rubenlucas 8:428ff7b7715d 569 {
rubenlucas 12:d19588a50fc7 570 float Kp = 11.1; // proportional gain
rubenlucas 12:d19588a50fc7 571 float Ki = 2.24;//integral gain
rubenlucas 12:d19588a50fc7 572 float Kd = 1.1; //derivative gain
rubenlucas 12:d19588a50fc7 573 static float ErrorIntegral = 0;
rubenlucas 12:d19588a50fc7 574 static float ErrorPrevious = Err2;
rubenlucas 8:428ff7b7715d 575 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
rubenlucas 8:428ff7b7715d 576
rubenlucas 8:428ff7b7715d 577 //Proportional part:
rubenlucas 12:d19588a50fc7 578 float u_k = Kp * Err2;
rubenlucas 8:428ff7b7715d 579
rubenlucas 8:428ff7b7715d 580 //Integral part:
rubenlucas 8:428ff7b7715d 581 ErrorIntegral = ErrorIntegral + Err2*Ts;
rubenlucas 12:d19588a50fc7 582 float u_i = Ki * ErrorIntegral;
rubenlucas 8:428ff7b7715d 583
rubenlucas 8:428ff7b7715d 584 //Derivative part:
rubenlucas 12:d19588a50fc7 585 float ErrorDerivative = (Err2 - ErrorPrevious)/Ts;
rubenlucas 12:d19588a50fc7 586 float FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
rubenlucas 12:d19588a50fc7 587 float u_d = Kd * FilteredErrorDerivative;
rubenlucas 8:428ff7b7715d 588 ErrorPrevious = Err2;
rubenlucas 8:428ff7b7715d 589
rubenlucas 8:428ff7b7715d 590 MotorValue2 = u_k + u_i + u_d; //This will become the MotorValue to set motor 2
rubenlucas 8:428ff7b7715d 591 }
rubenlucas 3:97cac1d5ba8a 592
rubenlucas 3:97cac1d5ba8a 593 // Main function for motorcontrol
rubenlucas 3:97cac1d5ba8a 594 void MotorControllers()
rubenlucas 8:428ff7b7715d 595 {
rubenlucas 8:428ff7b7715d 596 PID_Controller1(Error1);
rubenlucas 8:428ff7b7715d 597 PID_Controller2(Error2);
rubenlucas 8:428ff7b7715d 598
rubenlucas 2:c2ae5044ec82 599 }
rubenlucas 3:97cac1d5ba8a 600 //------------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 601
rubenlucas 2:c2ae5044ec82 602 //Ticker function set motorvalues
rubenlucas 3:97cac1d5ba8a 603 void SetMotors()
rubenlucas 2:c2ae5044ec82 604 {
rubenlucas 3:97cac1d5ba8a 605 // Motor 1
rubenlucas 8:428ff7b7715d 606 if (MotorValue1 >=0) { // set direction
rubenlucas 3:97cac1d5ba8a 607 DirectionPin1 = 1;
rubenlucas 8:428ff7b7715d 608 } else {
rubenlucas 3:97cac1d5ba8a 609 DirectionPin1 = 0;
rubenlucas 2:c2ae5044ec82 610 }
rubenlucas 8:428ff7b7715d 611
rubenlucas 8:428ff7b7715d 612 if (fabs(MotorValue1)>1) { // set duty cycle
rubenlucas 8:428ff7b7715d 613 PwmPin1 = 1;
rubenlucas 8:428ff7b7715d 614 } else {
rubenlucas 3:97cac1d5ba8a 615 PwmPin1 = fabs(MotorValue1);
rubenlucas 3:97cac1d5ba8a 616 }
rubenlucas 8:428ff7b7715d 617
rubenlucas 3:97cac1d5ba8a 618 // Motor 2
rubenlucas 8:428ff7b7715d 619 if (MotorValue2 >=0) { // set direction
rubenlucas 3:97cac1d5ba8a 620 DirectionPin2 = 1;
rubenlucas 8:428ff7b7715d 621 } else {
rubenlucas 3:97cac1d5ba8a 622 DirectionPin2 = 0;
rubenlucas 2:c2ae5044ec82 623 }
rubenlucas 8:428ff7b7715d 624
rubenlucas 8:428ff7b7715d 625 if (fabs(MotorValue2)>1) { // set duty cycle
rubenlucas 8:428ff7b7715d 626 PwmPin2 = 1;
rubenlucas 8:428ff7b7715d 627 } else {
rubenlucas 3:97cac1d5ba8a 628 PwmPin2 = fabs(MotorValue2);
rubenlucas 3:97cac1d5ba8a 629 }
rubenlucas 3:97cac1d5ba8a 630
rubenlucas 2:c2ae5044ec82 631 }
rubenlucas 2:c2ae5044ec82 632
rubenlucas 6:d7ae5f8fd460 633
rubenlucas 6:d7ae5f8fd460 634
rubenlucas 2:c2ae5044ec82 635
rubenlucas 2:c2ae5044ec82 636 //-----------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 637
rubenlucas 2:c2ae5044ec82 638
rubenlucas 2:c2ae5044ec82 639 // Execution function
rubenlucas 2:c2ae5044ec82 640 void LoopFunction()
rubenlucas 2:c2ae5044ec82 641 {
rubenlucas 6:d7ae5f8fd460 642 MeasureAll();
rubenlucas 6:d7ae5f8fd460 643 StateMachine();
rubenlucas 7:f4f0939f9df3 644 SetErrors();
rubenlucas 6:d7ae5f8fd460 645 MotorControllers();
rubenlucas 6:d7ae5f8fd460 646 SetMotors();
rubenlucas 7:f4f0939f9df3 647 PrintToScreen();
rubenlucas 13:3493825752ac 648
rubenlucas 2:c2ae5044ec82 649 }
rubenlucas 2:c2ae5044ec82 650
rubenlucas 2:c2ae5044ec82 651 int main()
rubenlucas 2:c2ae5044ec82 652 {
rubenlucas 8:428ff7b7715d 653 PwmPin1.period_us(60);
rubenlucas 8:428ff7b7715d 654 PwmPin2.period_us(60);
rubenlucas 2:c2ae5044ec82 655 pc.baud(115200);
rubenlucas 2:c2ae5044ec82 656 pc.printf("Hi I'm PTR, you can call me Peter!\r\n");
rubenlucas 3:97cac1d5ba8a 657 TimerLoop.start(); // start Timer
rubenlucas 2:c2ae5044ec82 658 CurrentState = Waiting; // set initial state as Waiting
rubenlucas 12:d19588a50fc7 659 CurrentOperationState = OPSet; //set initial state Operation mode
rubenlucas 13:3493825752ac 660 bqc1.add( &bq1 ).add( &bq2 ).add( &bq5 ); //make BiQuadChain EMG left
rubenlucas 13:3493825752ac 661 bqc2.add( &bq3 ).add( &bq4 ).add( &bq6 ); //make BiQuadChain EMG right
rubenlucas 2:c2ae5044ec82 662 TickerLoop.attach(&LoopFunction, 0.002f); //ticker for function calls 500 Hz
rubenlucas 8:428ff7b7715d 663
rubenlucas 8:428ff7b7715d 664 while (true) {
rubenlucas 8:428ff7b7715d 665 if (PrintFlag == true) {
rubenlucas 13:3493825752ac 666 pc.printf("BoolLeft = %i, BoolRight = %i, NormLeft = %f, MaxLeft = %f, NormRight = %f, MaxRight = %f ThresholdLeft = %f, ThresholdRight = %f\r",EMGBoolLeft,EMGBoolRight,NormLeft,MaxLeft,NormRight,MaxRight,ThresholdLeft,ThresholdRight);
rubenlucas 6:d7ae5f8fd460 667 }
rubenlucas 2:c2ae5044ec82 668 }
rubenlucas 8:428ff7b7715d 669
rubenlucas 2:c2ae5044ec82 670 }