Ruben Lucas / Mbed 2 deprecated StateMachinePTR

StateMachine PTR

Dependencies:   MODSERIAL QEI MovingAverage biquadFilter mbed

StateMachinePTR.cpp@3:97cac1d5ba8a, 2018-10-30 (annotated)

Committer:
rubenlucas
Date:
Tue Oct 30 13:48:54 2018 +0000
Revision:
3:97cac1d5ba8a
Parent:
2:c2ae5044ec82
Child:
4:4728763bbb44
Statemachine finished till IK, no states defined

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