Ruben Lucas / Mbed 2 deprecated StateMachinePTR

StateMachine PTR

Dependencies:   MODSERIAL QEI MovingAverage biquadFilter mbed

StateMachinePTR.cpp@8:428ff7b7715d, 2018-10-31 (annotated)

Committer:
rubenlucas
Date:
Wed Oct 31 16:06:57 2018 +0000
Revision:
8:428ff7b7715d
Parent:
7:f4f0939f9df3
Child:
9:3410f8dd6845
EMG works not properly. too much noise

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 8:428ff7b7715d 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 8:428ff7b7715d 27
rubenlucas 8:428ff7b7715d 28 // filters
rubenlucas 8:428ff7b7715d 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 6:d7ae5f8fd460 53 DigitalIn BUTSW2(SW2);
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 8:428ff7b7715d 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 8:428ff7b7715d 64
rubenlucas 8:428ff7b7715d 65 // variables
rubenlucas 7:f4f0939f9df3 66 //Motor calibration
rubenlucas 8:428ff7b7715d 67 bool NextMotorFlag = false;
rubenlucas 7:f4f0939f9df3 68
rubenlucas 3:97cac1d5ba8a 69 //EMG
rubenlucas 8:428ff7b7715d 70 bool EMGBoolLeft; // Boolean EMG 1 (left)
rubenlucas 8:428ff7b7715d 71 bool EMGBoolRight; // Boolean EMG 2 (right)
rubenlucas 8:428ff7b7715d 72 double LeftBicepsOut; // Final value left of processed EMG
rubenlucas 8:428ff7b7715d 73 double RightBicepsOut; // Final value right of processed EMG
rubenlucas 8:428ff7b7715d 74 double ThresholdLeft = 1; //Threshold to compare with value (start off with max value = 1)
rubenlucas 8:428ff7b7715d 75 double ThresholdRight = 1; //Threshold to compare with value (start off with max value = 1)
rubenlucas 3:97cac1d5ba8a 76
rubenlucas 6:d7ae5f8fd460 77 // Reference positions
rubenlucas 8:428ff7b7715d 78 float q1Ref = 0;
rubenlucas 8:428ff7b7715d 79 float q2Ref = 0;
rubenlucas 3:97cac1d5ba8a 80
rubenlucas 3:97cac1d5ba8a 81 //Motors
rubenlucas 8:428ff7b7715d 82 float q1 = 0; // Current angle of motor 1 (arm)
rubenlucas 8:428ff7b7715d 83 float q2 = 0; // Current angle of motor 2 (end-effector)
rubenlucas 8:428ff7b7715d 84 float MotorValue1; // Inputvalue to set motor 1
rubenlucas 8:428ff7b7715d 85 float MotorValue2; // Inputvalue to set motor 2
rubenlucas 3:97cac1d5ba8a 86
rubenlucas 3:97cac1d5ba8a 87 //Inverse kinematics
rubenlucas 8:428ff7b7715d 88 float VdesX; // Desired velocity in x direction
rubenlucas 8:428ff7b7715d 89 float VdesY; // Desired velocity in y direction
rubenlucas 8:428ff7b7715d 90 float Error1; // Difference in reference angle and current angle motor 1
rubenlucas 8:428ff7b7715d 91 float Error2; // Difference in reference angle and current angle motor 2
rubenlucas 3:97cac1d5ba8a 92
rubenlucas 6:d7ae5f8fd460 93 //Print to screen
rubenlucas 8:428ff7b7715d 94 int PrintFlag = false;
rubenlucas 8:428ff7b7715d 95 int CounterPrint = 0;
rubenlucas 2:c2ae5044ec82 96
rubenlucas 2:c2ae5044ec82 97
rubenlucas 3:97cac1d5ba8a 98 //------------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 99 //------------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 100
rubenlucas 6:d7ae5f8fd460 101 void PrintToScreen()
rubenlucas 6:d7ae5f8fd460 102 {
rubenlucas 8:428ff7b7715d 103 CounterPrint++;
rubenlucas 8:428ff7b7715d 104 if (CounterPrint == 100) {
rubenlucas 8:428ff7b7715d 105 PrintFlag = true;
rubenlucas 8:428ff7b7715d 106 CounterPrint = 0;
rubenlucas 8:428ff7b7715d 107 }
rubenlucas 6:d7ae5f8fd460 108 }
rubenlucas 6:d7ae5f8fd460 109
rubenlucas 6:d7ae5f8fd460 110 // Function to set motors off
rubenlucas 6:d7ae5f8fd460 111 void SetMotorsOff()
rubenlucas 6:d7ae5f8fd460 112 {
rubenlucas 6:d7ae5f8fd460 113 // Turn motors off
rubenlucas 8:428ff7b7715d 114 PwmPin1 = 0;
rubenlucas 6:d7ae5f8fd460 115 PwmPin2 = 0;
rubenlucas 8:428ff7b7715d 116
rubenlucas 6:d7ae5f8fd460 117 }
rubenlucas 6:d7ae5f8fd460 118
rubenlucas 3:97cac1d5ba8a 119 // Function for processing EMG
rubenlucas 8:428ff7b7715d 120 void ProcessingEMG()
rubenlucas 8:428ff7b7715d 121 {
rubenlucas 3:97cac1d5ba8a 122 //Filter Left Biceps
rubenlucas 3:97cac1d5ba8a 123 double LB_Filter_1 = bqc1.step(emg0.read()); //High Pass + Notch
rubenlucas 8:428ff7b7715d 124 double LB_Rectify = fabs(LB_Filter_1); //Rectify Signal
rubenlucas 3:97cac1d5ba8a 125 Movag_LB.Insert(LB_Rectify); //Moving Average
rubenlucas 3:97cac1d5ba8a 126 LeftBicepsOut = Movag_LB.GetAverage(); //Get final value
rubenlucas 8:428ff7b7715d 127
rubenlucas 3:97cac1d5ba8a 128 //Filter Right Biceps
rubenlucas 3:97cac1d5ba8a 129 double RB_Filter_1 = bqc2.step(emg1.read()); //High Pass + Notch
rubenlucas 3:97cac1d5ba8a 130 double RB_Rectify = fabs(RB_Filter_1); //Rectify Signal
rubenlucas 8:428ff7b7715d 131 Movag_RB.Insert(RB_Rectify); //Moving Average
rubenlucas 3:97cac1d5ba8a 132 RightBicepsOut = Movag_RB.GetAverage(); //Get final value
rubenlucas 8:428ff7b7715d 133
rubenlucas 8:428ff7b7715d 134 if (LeftBicepsOut > ThresholdLeft) {
rubenlucas 8:428ff7b7715d 135 EMGBoolLeft = true;
rubenlucas 3:97cac1d5ba8a 136 }
rubenlucas 8:428ff7b7715d 137 else {
rubenlucas 8:428ff7b7715d 138 EMGBoolLeft = false;
rubenlucas 8:428ff7b7715d 139 }
rubenlucas 8:428ff7b7715d 140 if (RightBicepsOut > ThresholdRight) {
rubenlucas 8:428ff7b7715d 141 EMGBoolRight = true;
rubenlucas 8:428ff7b7715d 142 }
rubenlucas 8:428ff7b7715d 143 else {
rubenlucas 8:428ff7b7715d 144 EMGBoolRight = false;
rubenlucas 8:428ff7b7715d 145 }
rubenlucas 8:428ff7b7715d 146
rubenlucas 8:428ff7b7715d 147 }
rubenlucas 3:97cac1d5ba8a 148
rubenlucas 3:97cac1d5ba8a 149 void MeasureAll()
rubenlucas 3:97cac1d5ba8a 150 {
rubenlucas 3:97cac1d5ba8a 151 //Processing and reading EMG
rubenlucas 3:97cac1d5ba8a 152 ProcessingEMG();
rubenlucas 3:97cac1d5ba8a 153
rubenlucas 3:97cac1d5ba8a 154 //Measure current motor angles
rubenlucas 7:f4f0939f9df3 155 float Counts1 = Encoder1.getPulses();
rubenlucas 7:f4f0939f9df3 156 float Counts2 = Encoder2.getPulses();
rubenlucas 7:f4f0939f9df3 157 q1 = Counts1/8400*(2*6.28318530718); //angle motor 1 (Times 2 to compensate for endcoding X4 --> X2)
rubenlucas 7:f4f0939f9df3 158 q2 = Counts2/8400*(2*6.28318530718); // angle motor 2 (Times 2 to compensate for endcoding X4 --> X2)
rubenlucas 8:428ff7b7715d 159
rubenlucas 7:f4f0939f9df3 160
rubenlucas 8:428ff7b7715d 161
rubenlucas 3:97cac1d5ba8a 162 }
rubenlucas 3:97cac1d5ba8a 163
rubenlucas 6:d7ae5f8fd460 164 // Function to set desired cartesian velocities of end-effector
rubenlucas 6:d7ae5f8fd460 165 void Vdesired()
rubenlucas 6:d7ae5f8fd460 166 {
rubenlucas 7:f4f0939f9df3 167 double Vconst = 0.05; // m/s (5 cm per second)
rubenlucas 6:d7ae5f8fd460 168 VdesX = EMGBoolLeft*Vconst; // Left biceps is X-direction
rubenlucas 6:d7ae5f8fd460 169 VdesY = -1*EMGBoolRight*Vconst; // Right biceps is minus Y-direction
rubenlucas 6:d7ae5f8fd460 170 }
rubenlucas 6:d7ae5f8fd460 171
rubenlucas 6:d7ae5f8fd460 172 // Inverse Kinematics
rubenlucas 6:d7ae5f8fd460 173 void InverseKinematics()
rubenlucas 6:d7ae5f8fd460 174 {
rubenlucas 6:d7ae5f8fd460 175 // matrix inverse Jacobian
rubenlucas 6:d7ae5f8fd460 176 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 8:428ff7b7715d 177 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 6:d7ae5f8fd460 178 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 6:d7ae5f8fd460 179 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 8:428ff7b7715d 180
rubenlucas 6:d7ae5f8fd460 181 //Gear Ratio's
rubenlucas 6:d7ae5f8fd460 182 double RatioGears = 134.0/30.0; //Gear Ratio for motor 1
rubenlucas 6:d7ae5f8fd460 183 double RatioPulleys = 87.4/27.5; // Gear Ratio for motor 2
rubenlucas 8:428ff7b7715d 184
rubenlucas 6:d7ae5f8fd460 185 double q1DotRef = (InvJac11*VdesX + InvJac12*VdesY)*RatioGears; //reference angular velocity motor 1
rubenlucas 6:d7ae5f8fd460 186 double q2DotRef = (InvJac21*VdesX + InvJac22*VdesY)*RatioPulleys; //reference angular velocity motor 2
rubenlucas 8:428ff7b7715d 187
rubenlucas 7:f4f0939f9df3 188 q1Ref = q1 + q1DotRef*Ts; // set new reference position motor angle 1
rubenlucas 7:f4f0939f9df3 189 q2Ref = q2 + q2DotRef*Ts; // set new reference position motor angle 1
rubenlucas 6:d7ae5f8fd460 190 }
rubenlucas 6:d7ae5f8fd460 191
rubenlucas 3:97cac1d5ba8a 192 //State machine
rubenlucas 3:97cac1d5ba8a 193 void StateMachine()
rubenlucas 2:c2ae5044ec82 194 {
rubenlucas 8:428ff7b7715d 195 switch (CurrentState) {
rubenlucas 2:c2ae5044ec82 196 case Waiting:
rubenlucas 6:d7ae5f8fd460 197 SetMotorsOff();
rubenlucas 7:f4f0939f9df3 198 Encoder1.reset();
rubenlucas 7:f4f0939f9df3 199 q1Ref = 0;
rubenlucas 7:f4f0939f9df3 200 q1 = 0;
rubenlucas 7:f4f0939f9df3 201 Error1 = 0;
rubenlucas 7:f4f0939f9df3 202 Encoder2.reset();
rubenlucas 7:f4f0939f9df3 203 q2Ref = 0;
rubenlucas 7:f4f0939f9df3 204 q2 = 0;
rubenlucas 7:f4f0939f9df3 205 Error2 = 0;
rubenlucas 8:428ff7b7715d 206
rubenlucas 2:c2ae5044ec82 207 LedRed = 0;
rubenlucas 2:c2ae5044ec82 208 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 209 LedBlue = 1; //Yellow
rubenlucas 8:428ff7b7715d 210
rubenlucas 8:428ff7b7715d 211 if (BUT2 == false) {
rubenlucas 5:cbcff21e74a0 212 CurrentState = EMGCal;
rubenlucas 6:d7ae5f8fd460 213 TimerLoop.reset();
rubenlucas 5:cbcff21e74a0 214 }
rubenlucas 8:428ff7b7715d 215
rubenlucas 8:428ff7b7715d 216 break;
rubenlucas 8:428ff7b7715d 217
rubenlucas 2:c2ae5044ec82 218 case Homing:
rubenlucas 2:c2ae5044ec82 219 LedRed = 0;
rubenlucas 2:c2ae5044ec82 220 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 221 LedBlue = 0; //White
rubenlucas 8:428ff7b7715d 222 if (BUT2 == false) {
rubenlucas 6:d7ae5f8fd460 223 CurrentState = Demo;
rubenlucas 8:428ff7b7715d 224 TimerLoop.reset();
rubenlucas 6:d7ae5f8fd460 225 }
rubenlucas 8:428ff7b7715d 226
rubenlucas 8:428ff7b7715d 227 if (BUT1 == false) {
rubenlucas 6:d7ae5f8fd460 228 CurrentState = Operation;
rubenlucas 8:428ff7b7715d 229 TimerLoop.reset();
rubenlucas 6:d7ae5f8fd460 230 }
rubenlucas 8:428ff7b7715d 231 break;
rubenlucas 8:428ff7b7715d 232
rubenlucas 2:c2ae5044ec82 233 case Emergency:
rubenlucas 2:c2ae5044ec82 234 LedRed = 0;
rubenlucas 2:c2ae5044ec82 235 LedGreen = 1;
rubenlucas 2:c2ae5044ec82 236 LedBlue = 1; //Red
rubenlucas 8:428ff7b7715d 237 break;
rubenlucas 8:428ff7b7715d 238
rubenlucas 2:c2ae5044ec82 239 case EMGCal:
rubenlucas 2:c2ae5044ec82 240 LedRed = 0;
rubenlucas 2:c2ae5044ec82 241 LedGreen = 1;
rubenlucas 2:c2ae5044ec82 242 LedBlue = 0; //Pink
rubenlucas 4:4728763bbb44 243 static double MaxLeft = 0;
rubenlucas 4:4728763bbb44 244 static double MaxRight = 0;
rubenlucas 8:428ff7b7715d 245
rubenlucas 8:428ff7b7715d 246 if(LeftBicepsOut > MaxLeft) {
rubenlucas 8:428ff7b7715d 247 MaxLeft = LeftBicepsOut;
rubenlucas 8:428ff7b7715d 248 ThresholdLeft = abs(0.2000*MaxLeft);
rubenlucas 8:428ff7b7715d 249 TimerLoop.reset();
rubenlucas 8:428ff7b7715d 250 }
rubenlucas 8:428ff7b7715d 251
rubenlucas 8:428ff7b7715d 252 if(RightBicepsOut > MaxRight) {
rubenlucas 8:428ff7b7715d 253 MaxRight = RightBicepsOut;
rubenlucas 8:428ff7b7715d 254 ThresholdRight = abs(0.2000*MaxRight);
rubenlucas 4:4728763bbb44 255 TimerLoop.reset();
rubenlucas 4:4728763bbb44 256 }
rubenlucas 8:428ff7b7715d 257
rubenlucas 8:428ff7b7715d 258 if (BUT1 == false) {
rubenlucas 8:428ff7b7715d 259 //ThresholdLeft = abs(0.15000*MaxLeft);
rubenlucas 8:428ff7b7715d 260 //ThresholdRight = abs(0.15000*MaxRight);
rubenlucas 8:428ff7b7715d 261 }
rubenlucas 8:428ff7b7715d 262 if ((ThresholdLeft<0.9) && (ThresholdRight <0.9) && (TimerLoop.read() >= 5.0)) {
rubenlucas 4:4728763bbb44 263 TimerLoop.reset();
rubenlucas 8:428ff7b7715d 264 CurrentState = MotorCal;
rubenlucas 4:4728763bbb44 265 }
rubenlucas 8:428ff7b7715d 266 break;
rubenlucas 8:428ff7b7715d 267
rubenlucas 2:c2ae5044ec82 268 case MotorCal:
rubenlucas 2:c2ae5044ec82 269 LedRed = 1;
rubenlucas 2:c2ae5044ec82 270 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 271 LedBlue = 0; //Turqoise
rubenlucas 8:428ff7b7715d 272 if (NextMotorFlag == false) {
rubenlucas 8:428ff7b7715d 273 if (BUT1==false) {
rubenlucas 8:428ff7b7715d 274 q1Ref += 0.0005*(6.2830);
rubenlucas 7:f4f0939f9df3 275 }
rubenlucas 8:428ff7b7715d 276 if (BUTSW3 == false) {
rubenlucas 8:428ff7b7715d 277 q1Ref = 0;
rubenlucas 8:428ff7b7715d 278 Encoder1.reset();
rubenlucas 8:428ff7b7715d 279 }
rubenlucas 8:428ff7b7715d 280 if (BUT2 == false) {
rubenlucas 8:428ff7b7715d 281 q1Ref -=0.0005*(6.2830);
rubenlucas 7:f4f0939f9df3 282 }
rubenlucas 8:428ff7b7715d 283 if (BUTSW2 == false) {
rubenlucas 8:428ff7b7715d 284 if (q1Ref<=0.733*(6.2830)) {
rubenlucas 8:428ff7b7715d 285 q1Ref +=0.0005*(6.2830);
rubenlucas 8:428ff7b7715d 286 } else {
rubenlucas 8:428ff7b7715d 287 TimerLoop.reset();
rubenlucas 8:428ff7b7715d 288 NextMotorFlag = true;
rubenlucas 8:428ff7b7715d 289 }
rubenlucas 8:428ff7b7715d 290
rubenlucas 8:428ff7b7715d 291 } //End of if (BUTSW2 == false)
rubenlucas 8:428ff7b7715d 292 } //End of if (NextMotorFlag == false)
rubenlucas 8:428ff7b7715d 293
rubenlucas 8:428ff7b7715d 294 else if ((NextMotorFlag == true) && (TimerLoop.read()>= 2)) {
rubenlucas 8:428ff7b7715d 295 if (BUT1==false) {
rubenlucas 8:428ff7b7715d 296 q2Ref += 0.0005*(6.2830);
rubenlucas 8:428ff7b7715d 297 }
rubenlucas 8:428ff7b7715d 298 if (BUTSW3 == false) {
rubenlucas 8:428ff7b7715d 299 q2Ref = 0;
rubenlucas 8:428ff7b7715d 300 Encoder2.reset();
rubenlucas 7:f4f0939f9df3 301 }
rubenlucas 8:428ff7b7715d 302 if (BUT2 == false) {
rubenlucas 8:428ff7b7715d 303 q2Ref -= 0.0005*(6.2830);
rubenlucas 7:f4f0939f9df3 304 }
rubenlucas 8:428ff7b7715d 305 if (BUTSW2 == false) {
rubenlucas 8:428ff7b7715d 306 if (q2Ref>=-0.52*(6.2830)) {
rubenlucas 8:428ff7b7715d 307 q2Ref -=0.0005*(6.2830);
rubenlucas 8:428ff7b7715d 308 } else {
rubenlucas 8:428ff7b7715d 309 CurrentState = Homing;
rubenlucas 8:428ff7b7715d 310 Encoder1.reset();
rubenlucas 8:428ff7b7715d 311 Encoder2.reset();
rubenlucas 8:428ff7b7715d 312 q1Ref = 0;
rubenlucas 8:428ff7b7715d 313 q2Ref = 0;
rubenlucas 8:428ff7b7715d 314 TimerLoop.reset();
rubenlucas 8:428ff7b7715d 315 }
rubenlucas 8:428ff7b7715d 316 } // end of if (BUTSW2) statement
rubenlucas 8:428ff7b7715d 317 }// end of else if statement
rubenlucas 8:428ff7b7715d 318
rubenlucas 8:428ff7b7715d 319 break;
rubenlucas 8:428ff7b7715d 320
rubenlucas 2:c2ae5044ec82 321 case Operation:
rubenlucas 2:c2ae5044ec82 322 LedRed = 1;
rubenlucas 2:c2ae5044ec82 323 LedGreen = 0;
rubenlucas 2:c2ae5044ec82 324 LedBlue = 1; //Green
rubenlucas 8:428ff7b7715d 325 break;
rubenlucas 8:428ff7b7715d 326
rubenlucas 2:c2ae5044ec82 327 case Demo:
rubenlucas 2:c2ae5044ec82 328 LedRed = 1;
rubenlucas 2:c2ae5044ec82 329 LedGreen = 1;
rubenlucas 8:428ff7b7715d 330 LedBlue = 0; //Blue
rubenlucas 8:428ff7b7715d 331
rubenlucas 7:f4f0939f9df3 332 //if (TimerLoop.read() <= 5.0)
rubenlucas 7:f4f0939f9df3 333 //{
rubenlucas 8:428ff7b7715d 334 // if((EMGBoolLeft == true) || (EMGBoolRight == true))
rubenlucas 8:428ff7b7715d 335 //{
rubenlucas 8:428ff7b7715d 336 TimerLoop.reset();
rubenlucas 8:428ff7b7715d 337 Vdesired();
rubenlucas 8:428ff7b7715d 338 InverseKinematics();
rubenlucas 8:428ff7b7715d 339 //}
rubenlucas 7:f4f0939f9df3 340 //}
rubenlucas 7:f4f0939f9df3 341 //else
rubenlucas 7:f4f0939f9df3 342 //{
rubenlucas 8:428ff7b7715d 343 // q1Ref = 0;
rubenlucas 8:428ff7b7715d 344 // q2Ref = 0;
rubenlucas 8:428ff7b7715d 345 // Error1 = q1Ref - q1;
rubenlucas 8:428ff7b7715d 346 // Error2 = q2Ref - q2;
rubenlucas 8:428ff7b7715d 347 // if ((Error1 <= 0.1) && (Error2 <= 0.1))
rubenlucas 8:428ff7b7715d 348 //{
rubenlucas 8:428ff7b7715d 349 // TimerLoop.reset();
rubenlucas 8:428ff7b7715d 350 // }
rubenlucas 8:428ff7b7715d 351
rubenlucas 8:428ff7b7715d 352 break;
rubenlucas 8:428ff7b7715d 353
rubenlucas 2:c2ae5044ec82 354
rubenlucas 7:f4f0939f9df3 355 } //End of switch
rubenlucas 7:f4f0939f9df3 356 } // End of stateMachine function
rubenlucas 2:c2ae5044ec82 357
rubenlucas 3:97cac1d5ba8a 358 //------------------------------------------------------------------------------
rubenlucas 4:4728763bbb44 359
rubenlucas 7:f4f0939f9df3 360 void SetErrors()
rubenlucas 7:f4f0939f9df3 361 {
rubenlucas 7:f4f0939f9df3 362 Error1 = q1Ref - q1;
rubenlucas 7:f4f0939f9df3 363 Error2 = q2Ref - q2;
rubenlucas 7:f4f0939f9df3 364 }
rubenlucas 3:97cac1d5ba8a 365 //------------------------------------------------------------------------------
rubenlucas 3:97cac1d5ba8a 366 // controller motor 1
rubenlucas 8:428ff7b7715d 367 void PID_Controller1(double Err1)
rubenlucas 8:428ff7b7715d 368 {
rubenlucas 8:428ff7b7715d 369 double Kp = 11.1; // proportional gain
rubenlucas 8:428ff7b7715d 370 double Ki = 2.24;//integral gain
rubenlucas 8:428ff7b7715d 371 double Kd = 1.1; //derivative gain
rubenlucas 8:428ff7b7715d 372 static double ErrorIntegral = 0;
rubenlucas 8:428ff7b7715d 373 static double ErrorPrevious = Err1;
rubenlucas 8:428ff7b7715d 374 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
rubenlucas 8:428ff7b7715d 375
rubenlucas 8:428ff7b7715d 376 //Proportional part:
rubenlucas 8:428ff7b7715d 377 double u_k = Kp * Err1;
rubenlucas 8:428ff7b7715d 378
rubenlucas 8:428ff7b7715d 379 //Integral part:
rubenlucas 8:428ff7b7715d 380 ErrorIntegral = ErrorIntegral + Err1*Ts;
rubenlucas 8:428ff7b7715d 381 double u_i = Ki * ErrorIntegral;
rubenlucas 8:428ff7b7715d 382
rubenlucas 8:428ff7b7715d 383 //Derivative part:
rubenlucas 8:428ff7b7715d 384 double ErrorDerivative = (Err1 - ErrorPrevious)/Ts;
rubenlucas 8:428ff7b7715d 385 double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
rubenlucas 8:428ff7b7715d 386 double u_d = Kd * FilteredErrorDerivative;
rubenlucas 8:428ff7b7715d 387 ErrorPrevious = Err1;
rubenlucas 8:428ff7b7715d 388
rubenlucas 8:428ff7b7715d 389 MotorValue1 = u_k + u_i + u_d; //This will become the MotorValue to set motor 1
rubenlucas 8:428ff7b7715d 390 }
rubenlucas 8:428ff7b7715d 391
rubenlucas 3:97cac1d5ba8a 392 // controller motor 2
rubenlucas 8:428ff7b7715d 393 void PID_Controller2(double Err2)
rubenlucas 8:428ff7b7715d 394 {
rubenlucas 8:428ff7b7715d 395 double Kp = 11.1; // proportional gain
rubenlucas 8:428ff7b7715d 396 double Ki = 22.81;//integral gain
rubenlucas 8:428ff7b7715d 397 double Kd = 1.7; //derivative gain
rubenlucas 8:428ff7b7715d 398 static double ErrorIntegral = 0;
rubenlucas 8:428ff7b7715d 399 static double ErrorPrevious = Err2;
rubenlucas 8:428ff7b7715d 400 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
rubenlucas 8:428ff7b7715d 401
rubenlucas 8:428ff7b7715d 402 //Proportional part:
rubenlucas 8:428ff7b7715d 403 double u_k = Kp * Err2;
rubenlucas 8:428ff7b7715d 404
rubenlucas 8:428ff7b7715d 405 //Integral part:
rubenlucas 8:428ff7b7715d 406 ErrorIntegral = ErrorIntegral + Err2*Ts;
rubenlucas 8:428ff7b7715d 407 double u_i = Ki * ErrorIntegral;
rubenlucas 8:428ff7b7715d 408
rubenlucas 8:428ff7b7715d 409 //Derivative part:
rubenlucas 8:428ff7b7715d 410 double ErrorDerivative = (Err2 - ErrorPrevious)/Ts;
rubenlucas 8:428ff7b7715d 411 double FilteredErrorDerivative = LowPassFilter.step(ErrorDerivative);
rubenlucas 8:428ff7b7715d 412 double u_d = Kd * FilteredErrorDerivative;
rubenlucas 8:428ff7b7715d 413 ErrorPrevious = Err2;
rubenlucas 8:428ff7b7715d 414
rubenlucas 8:428ff7b7715d 415 MotorValue2 = u_k + u_i + u_d; //This will become the MotorValue to set motor 2
rubenlucas 8:428ff7b7715d 416 }
rubenlucas 3:97cac1d5ba8a 417
rubenlucas 3:97cac1d5ba8a 418 // Main function for motorcontrol
rubenlucas 3:97cac1d5ba8a 419 void MotorControllers()
rubenlucas 8:428ff7b7715d 420 {
rubenlucas 8:428ff7b7715d 421 PID_Controller1(Error1);
rubenlucas 8:428ff7b7715d 422 PID_Controller2(Error2);
rubenlucas 8:428ff7b7715d 423
rubenlucas 2:c2ae5044ec82 424 }
rubenlucas 3:97cac1d5ba8a 425 //------------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 426
rubenlucas 2:c2ae5044ec82 427 //Ticker function set motorvalues
rubenlucas 3:97cac1d5ba8a 428 void SetMotors()
rubenlucas 2:c2ae5044ec82 429 {
rubenlucas 3:97cac1d5ba8a 430 // Motor 1
rubenlucas 8:428ff7b7715d 431 if (MotorValue1 >=0) { // set direction
rubenlucas 3:97cac1d5ba8a 432 DirectionPin1 = 1;
rubenlucas 8:428ff7b7715d 433 } else {
rubenlucas 3:97cac1d5ba8a 434 DirectionPin1 = 0;
rubenlucas 2:c2ae5044ec82 435 }
rubenlucas 8:428ff7b7715d 436
rubenlucas 8:428ff7b7715d 437 if (fabs(MotorValue1)>1) { // set duty cycle
rubenlucas 8:428ff7b7715d 438 PwmPin1 = 1;
rubenlucas 8:428ff7b7715d 439 } else {
rubenlucas 3:97cac1d5ba8a 440 PwmPin1 = fabs(MotorValue1);
rubenlucas 3:97cac1d5ba8a 441 }
rubenlucas 8:428ff7b7715d 442
rubenlucas 3:97cac1d5ba8a 443 // Motor 2
rubenlucas 8:428ff7b7715d 444 if (MotorValue2 >=0) { // set direction
rubenlucas 3:97cac1d5ba8a 445 DirectionPin2 = 1;
rubenlucas 8:428ff7b7715d 446 } else {
rubenlucas 3:97cac1d5ba8a 447 DirectionPin2 = 0;
rubenlucas 2:c2ae5044ec82 448 }
rubenlucas 8:428ff7b7715d 449
rubenlucas 8:428ff7b7715d 450 if (fabs(MotorValue2)>1) { // set duty cycle
rubenlucas 8:428ff7b7715d 451 PwmPin2 = 1;
rubenlucas 8:428ff7b7715d 452 } else {
rubenlucas 3:97cac1d5ba8a 453 PwmPin2 = fabs(MotorValue2);
rubenlucas 3:97cac1d5ba8a 454 }
rubenlucas 3:97cac1d5ba8a 455
rubenlucas 2:c2ae5044ec82 456 }
rubenlucas 2:c2ae5044ec82 457
rubenlucas 6:d7ae5f8fd460 458
rubenlucas 6:d7ae5f8fd460 459
rubenlucas 2:c2ae5044ec82 460
rubenlucas 2:c2ae5044ec82 461 //-----------------------------------------------------------------------------
rubenlucas 2:c2ae5044ec82 462
rubenlucas 2:c2ae5044ec82 463
rubenlucas 2:c2ae5044ec82 464 // Execution function
rubenlucas 2:c2ae5044ec82 465 void LoopFunction()
rubenlucas 2:c2ae5044ec82 466 {
rubenlucas 6:d7ae5f8fd460 467 MeasureAll();
rubenlucas 6:d7ae5f8fd460 468 StateMachine();
rubenlucas 7:f4f0939f9df3 469 SetErrors();
rubenlucas 6:d7ae5f8fd460 470 MotorControllers();
rubenlucas 6:d7ae5f8fd460 471 SetMotors();
rubenlucas 7:f4f0939f9df3 472 PrintToScreen();
rubenlucas 2:c2ae5044ec82 473 }
rubenlucas 2:c2ae5044ec82 474
rubenlucas 2:c2ae5044ec82 475 int main()
rubenlucas 2:c2ae5044ec82 476 {
rubenlucas 8:428ff7b7715d 477 PwmPin1.period_us(60);
rubenlucas 8:428ff7b7715d 478 PwmPin2.period_us(60);
rubenlucas 2:c2ae5044ec82 479 pc.baud(115200);
rubenlucas 2:c2ae5044ec82 480 pc.printf("Hi I'm PTR, you can call me Peter!\r\n");
rubenlucas 3:97cac1d5ba8a 481 TimerLoop.start(); // start Timer
rubenlucas 2:c2ae5044ec82 482 CurrentState = Waiting; // set initial state as Waiting
rubenlucas 3:97cac1d5ba8a 483 bqc1.add( &bq1 ).add( &bq2 ); //make BiQuadChain EMG left
rubenlucas 3:97cac1d5ba8a 484 bqc2.add( &bq3 ).add( &bq4 ); //make BiQuadChain EMG right
rubenlucas 2:c2ae5044ec82 485 TickerLoop.attach(&LoopFunction, 0.002f); //ticker for function calls 500 Hz
rubenlucas 8:428ff7b7715d 486
rubenlucas 8:428ff7b7715d 487 while (true) {
rubenlucas 8:428ff7b7715d 488 if (PrintFlag == true) {
rubenlucas 8:428ff7b7715d 489 pc.printf("THleft = %f,THRight = %f,EMG left = %i, EMG right = %i, MOVAG Left = %f, MOVAG Right = %f\r",ThresholdLeft,ThresholdRight,EMGBoolLeft,EMGBoolRight,LeftBicepsOut, RightBicepsOut);
rubenlucas 6:d7ae5f8fd460 490 }
rubenlucas 2:c2ae5044ec82 491 }
rubenlucas 8:428ff7b7715d 492
rubenlucas 2:c2ae5044ec82 493 }