Ruben Lucas / Mbed 2 deprecated StateMachinePTR

StateMachine PTR

Dependencies:   MODSERIAL QEI MovingAverage biquadFilter mbed

StateMachinePTR.cpp@11:2ffae0f2110a, 2018-11-01 (annotated)

Committer:
rubenlucas
Date:
Thu Nov 01 09:08:09 2018 +0000
Revision:
11:2ffae0f2110a
Parent:
10:9c18b5d08c24
Child:
12:d19588a50fc7
Inverse Kinematics works  alsmost perfect, The errors are changed to the errors of the motors.

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