Ruben Lucas / Mbed 2 deprecated StateMachinePTR

StateMachine PTR

Dependencies:   MODSERIAL QEI MovingAverage biquadFilter mbed

StateMachinePTR.cpp@7:f4f0939f9df3, 2018-10-31 (annotated)

Committer:
rubenlucas
Date:
Wed Oct 31 13:40:56 2018 +0000
Revision:
7:f4f0939f9df3
Parent:
6:d7ae5f8fd460
Child:
8:428ff7b7715d
huidig

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