Gerhard Berman / Mbed 2 deprecated prog_inversekin

First trial for Inverse Kinematics Feedforward implementation. No errors, not yet tested with board

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_pract3_3_PI_controller by Gerhard Berman

main.cpp@8:935abf8ecc27, 2016-10-19 (annotated)

Committer:
GerhardBerman
Date:
Wed Oct 19 14:16:10 2016 +0000
Revision:
8:935abf8ecc27
Parent:
7:2f74dfd1d411
Child:
9:e4c34f5665a0
Improved if-else loops for both biceps signals

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GerhardBerman 0:43160ef59f9f 1 #include "mbed.h"
GerhardBerman 0:43160ef59f9f 2 #include <math.h>
GerhardBerman 0:43160ef59f9f 3 #include "MODSERIAL.h"
GerhardBerman 0:43160ef59f9f 4 #include "QEI.h"
GerhardBerman 0:43160ef59f9f 5 #include "HIDScope.h"
GerhardBerman 0:43160ef59f9f 6 #include "BiQuad.h"
GerhardBerman 0:43160ef59f9f 7
GerhardBerman 0:43160ef59f9f 8 //set pins
GerhardBerman 0:43160ef59f9f 9 DigitalIn encoder1A (D13); //Channel A van Encoder 1
GerhardBerman 0:43160ef59f9f 10 DigitalIn encoder1B (D12); //Channel B van Encoder 1
GerhardBerman 0:43160ef59f9f 11 DigitalOut led1 (D11);
GerhardBerman 0:43160ef59f9f 12 DigitalOut led2 (D10);
GerhardBerman 3:8caef4872b0c 13 AnalogIn potMeter1(A2);
GerhardBerman 0:43160ef59f9f 14 AnalogIn potMeter2(A1);
GerhardBerman 0:43160ef59f9f 15 DigitalOut motor1DirectionPin(D7);
GerhardBerman 0:43160ef59f9f 16 PwmOut motor1MagnitudePin(D6);
GerhardBerman 7:2f74dfd1d411 17 DigitalOut motor2DirectionPin(D4);
GerhardBerman 7:2f74dfd1d411 18 PwmOut motor2MagnitudePin(D5);
GerhardBerman 7:2f74dfd1d411 19 DigitalIn button1(D8);
GerhardBerman 7:2f74dfd1d411 20 DigitalIn button2(D9);
GerhardBerman 0:43160ef59f9f 21
GerhardBerman 7:2f74dfd1d411 22 //library settings
GerhardBerman 0:43160ef59f9f 23 Serial pc(USBTX,USBRX);
GerhardBerman 3:8caef4872b0c 24 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
GerhardBerman 6:3c4f3f2ce54f 25 HIDScope scope(3);
GerhardBerman 0:43160ef59f9f 26
GerhardBerman 7:2f74dfd1d411 27 //set initial conditions
GerhardBerman 7:2f74dfd1d411 28 float error1_prev = 0;
GerhardBerman 7:2f74dfd1d411 29 float error2_prev = 0;
GerhardBerman 7:2f74dfd1d411 30 float IntError1 = 0;
GerhardBerman 7:2f74dfd1d411 31 float IntError2 = 0;
GerhardBerman 7:2f74dfd1d411 32 float q1 = 0;
GerhardBerman 7:2f74dfd1d411 33 float q2 = 0;
GerhardBerman 7:2f74dfd1d411 34 float q1_dot;
GerhardBerman 7:2f74dfd1d411 35 float q2_dot;
GerhardBerman 4:19e376d31380 36
GerhardBerman 7:2f74dfd1d411 37 //set constant or variable values
GerhardBerman 7:2f74dfd1d411 38 int counts1 = 0;
GerhardBerman 7:2f74dfd1d411 39 int counts2 = 0;
GerhardBerman 7:2f74dfd1d411 40 int counts1Prev = 0;
GerhardBerman 7:2f74dfd1d411 41 int counts2Prev = 0;
GerhardBerman 7:2f74dfd1d411 42 double DerivativeCounts;
GerhardBerman 7:2f74dfd1d411 43 float x0 = 1.0;
GerhardBerman 7:2f74dfd1d411 44 float L0 = 1.0;
GerhardBerman 7:2f74dfd1d411 45 float L1 = 1.0;
GerhardBerman 7:2f74dfd1d411 46 float dx;
GerhardBerman 7:2f74dfd1d411 47 float dy;
GerhardBerman 7:2f74dfd1d411 48 float dy_stampdown = 0.05; //5 cm movement downward to stamp
GerhardBerman 7:2f74dfd1d411 49
GerhardBerman 4:19e376d31380 50 float t_sample = 0.01; //seconds
GerhardBerman 0:43160ef59f9f 51 float referenceVelocity = 0;
GerhardBerman 3:8caef4872b0c 52 float bqcDerivativeCounts = 0;
GerhardBerman 3:8caef4872b0c 53 const float PI = 3.141592653589793;
GerhardBerman 3:8caef4872b0c 54 const int cw = 0; //values for cw and ccw are inverted!! cw=0 and ccw=1
GerhardBerman 3:8caef4872b0c 55 const int ccw = 1;
GerhardBerman 0:43160ef59f9f 56
GerhardBerman 0:43160ef59f9f 57 //set BiQuad
GerhardBerman 0:43160ef59f9f 58 BiQuadChain bqc;
GerhardBerman 0:43160ef59f9f 59 BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB
GerhardBerman 0:43160ef59f9f 60 BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762);
GerhardBerman 0:43160ef59f9f 61
GerhardBerman 0:43160ef59f9f 62 //set go-Ticker settings
GerhardBerman 3:8caef4872b0c 63 volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
GerhardBerman 3:8caef4872b0c 64 void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
GerhardBerman 3:8caef4872b0c 65 void BiQuadTicker_act(){BiQuadTicker_go=true;};
GerhardBerman 3:8caef4872b0c 66 void FeedbackTicker_act(){FeedbackTicker_go=true;};
GerhardBerman 3:8caef4872b0c 67 void TimeTracker_act(){TimeTracker_go=true;};
GerhardBerman 3:8caef4872b0c 68 //void sampleT_act(){sampleT_go=true;};
GerhardBerman 3:8caef4872b0c 69
GerhardBerman 3:8caef4872b0c 70 //define encoder counts and degrees
GerhardBerman 7:2f74dfd1d411 71 QEI Encoder1(D12, D13, NC, 32); // turns on encoder
GerhardBerman 7:2f74dfd1d411 72 QEI Encoder2(D14, D15, NC, 32); // turns on encoder
GerhardBerman 4:19e376d31380 73 const int counts_per_revolution = 4200; //counts per motor axis revolution
GerhardBerman 4:19e376d31380 74 const int inverse_gear_ratio = 131;
GerhardBerman 4:19e376d31380 75 //const float motor_axial_resolution = counts_per_revolution/(2*PI);
GerhardBerman 4:19e376d31380 76 const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis
GerhardBerman 3:8caef4872b0c 77
GerhardBerman 7:2f74dfd1d411 78 float GetReferenceKinematics1(){
GerhardBerman 7:2f74dfd1d411 79
GerhardBerman 7:2f74dfd1d411 80 //get joint positions q from encoder
GerhardBerman 7:2f74dfd1d411 81 float Encoder1Position = counts1/resolution; //position in radians, encoder axis
GerhardBerman 7:2f74dfd1d411 82 float q1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 7:2f74dfd1d411 83
GerhardBerman 7:2f74dfd1d411 84 //float Encoder2Position = counts2/resolution; //position in radians, encoder axis
GerhardBerman 7:2f74dfd1d411 85 //float q2 = Encoder2Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 7:2f74dfd1d411 86
GerhardBerman 7:2f74dfd1d411 87 //NOTNECESSARY calculate end effector position with Brockett
GerhardBerman 7:2f74dfd1d411 88
GerhardBerman 7:2f74dfd1d411 89 //NOTNECESSARY get desired position Pe* from EMG(?)
GerhardBerman 7:2f74dfd1d411 90
GerhardBerman 7:2f74dfd1d411 91 //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
GerhardBerman 7:2f74dfd1d411 92 float biceps1 = button1.read();
GerhardBerman 7:2f74dfd1d411 93 float biceps2 = button2.read();
GerhardBerman 8:935abf8ecc27 94 if (biceps1 > 0 && biceps2 > 0){
GerhardBerman 8:935abf8ecc27 95 //both arms activated: stamp moves down
GerhardBerman 8:935abf8ecc27 96 dx = 0;
GerhardBerman 8:935abf8ecc27 97 dy = dy_stampdown; //into stamping vertical position?? ~the stamp down action
GerhardBerman 8:935abf8ecc27 98 wait(1);
GerhardBerman 8:935abf8ecc27 99 dy = -(dy_stampdown); //reset vertical position
GerhardBerman 8:935abf8ecc27 100 }
GerhardBerman 8:935abf8ecc27 101 else if (biceps1 > 0 && biceps2 <= 0){
GerhardBerman 8:935abf8ecc27 102 //arm 1 activated, move left
GerhardBerman 8:935abf8ecc27 103 dx = -biceps1;
GerhardBerman 8:935abf8ecc27 104 dy = 0;
GerhardBerman 8:935abf8ecc27 105 }
GerhardBerman 8:935abf8ecc27 106 else if (biceps1 <= 0 && biceps2 > 0){
GerhardBerman 8:935abf8ecc27 107 //arm 1 activated, move left
GerhardBerman 8:935abf8ecc27 108 dx = biceps2;
GerhardBerman 8:935abf8ecc27 109 dy = 0;
GerhardBerman 8:935abf8ecc27 110 }
GerhardBerman 8:935abf8ecc27 111 else{
GerhardBerman 8:935abf8ecc27 112 dx=0;
GerhardBerman 8:935abf8ecc27 113 dy=0;
GerhardBerman 8:935abf8ecc27 114 }
GerhardBerman 8:935abf8ecc27 115
GerhardBerman 7:2f74dfd1d411 116 //get joint angles change q_dot = Jpseudo * TwistEndEff (Matlab)
GerhardBerman 7:2f74dfd1d411 117 float q1_dot = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
GerhardBerman 7:2f74dfd1d411 118
GerhardBerman 7:2f74dfd1d411 119 //update joint angles
GerhardBerman 7:2f74dfd1d411 120 q1 = q1 + q1_dot;
GerhardBerman 7:2f74dfd1d411 121 return q1_dot;
GerhardBerman 7:2f74dfd1d411 122 }
GerhardBerman 7:2f74dfd1d411 123
GerhardBerman 7:2f74dfd1d411 124 float GetReferenceKinematics2(){
GerhardBerman 7:2f74dfd1d411 125
GerhardBerman 7:2f74dfd1d411 126 //get joint positions q from encoder
GerhardBerman 7:2f74dfd1d411 127 float Encoder1Position = counts1/resolution; //position in radians, encoder axis
GerhardBerman 7:2f74dfd1d411 128 float q1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 7:2f74dfd1d411 129
GerhardBerman 7:2f74dfd1d411 130 float Encoder2Position = counts2/resolution; //position in radians, encoder axis
GerhardBerman 7:2f74dfd1d411 131 float q2 = Encoder2Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 7:2f74dfd1d411 132
GerhardBerman 7:2f74dfd1d411 133 //NOTNECESSARY calculate end effector position with Brockett
GerhardBerman 7:2f74dfd1d411 134
GerhardBerman 7:2f74dfd1d411 135 //NOTNECESSARY get desired position Pe* from EMG(?)
GerhardBerman 7:2f74dfd1d411 136
GerhardBerman 7:2f74dfd1d411 137 //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
GerhardBerman 7:2f74dfd1d411 138 float biceps1 = button1.read();
GerhardBerman 7:2f74dfd1d411 139 float biceps2 = button2.read();
GerhardBerman 7:2f74dfd1d411 140 while (biceps1 > 0){
GerhardBerman 7:2f74dfd1d411 141 if (biceps2 > 0){ //both arms activated: stamp moves down
GerhardBerman 7:2f74dfd1d411 142 dx = 0;
GerhardBerman 7:2f74dfd1d411 143 dy = dy_stampdown; //into stamping vertical position?? ~the stamp down action
GerhardBerman 7:2f74dfd1d411 144 wait(1);
GerhardBerman 7:2f74dfd1d411 145 dy = -(dy_stampdown); //reset vertical position
GerhardBerman 7:2f74dfd1d411 146 }
GerhardBerman 7:2f74dfd1d411 147 else{ //left arm activated
GerhardBerman 7:2f74dfd1d411 148 dx = biceps1;
GerhardBerman 7:2f74dfd1d411 149 dy = 0;
GerhardBerman 7:2f74dfd1d411 150 }
GerhardBerman 7:2f74dfd1d411 151 while (biceps2 > 0){
GerhardBerman 7:2f74dfd1d411 152 if (biceps1 <= 0){ //right arm activated
GerhardBerman 7:2f74dfd1d411 153 dx = -biceps2;
GerhardBerman 7:2f74dfd1d411 154 dy = 0;
GerhardBerman 7:2f74dfd1d411 155 }
GerhardBerman 7:2f74dfd1d411 156 }
GerhardBerman 7:2f74dfd1d411 157
GerhardBerman 7:2f74dfd1d411 158 //get joint angles change q_dot = Jpseudo * TwistEndEff; (Matlab)
GerhardBerman 7:2f74dfd1d411 159 float q2_dot = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
GerhardBerman 7:2f74dfd1d411 160
GerhardBerman 7:2f74dfd1d411 161 //update joint angles
GerhardBerman 7:2f74dfd1d411 162 q2 = q2 + q2_dot;
GerhardBerman 7:2f74dfd1d411 163 }
GerhardBerman 7:2f74dfd1d411 164 return q2_dot;
GerhardBerman 7:2f74dfd1d411 165 }
GerhardBerman 7:2f74dfd1d411 166
GerhardBerman 7:2f74dfd1d411 167 /*
GerhardBerman 7:2f74dfd1d411 168 float GetReferencePosition(){
GerhardBerman 3:8caef4872b0c 169 // Returns reference position in rad.
GerhardBerman 3:8caef4872b0c 170 // Positive value means clockwise rotation.
GerhardBerman 3:8caef4872b0c 171 const float maxPosition = 2*PI; //6.283185307179586; // in radians
GerhardBerman 3:8caef4872b0c 172 float Potmeter1 = potMeter1.read();
GerhardBerman 7:2f74dfd1d411 173 float referencePosition1 = Potmeter1 * maxPosition; //Potmeter1 * maxPosition; //refpos in radians
GerhardBerman 3:8caef4872b0c 174 pc.printf("Max Position: %f rad \r\n", maxPosition);
GerhardBerman 4:19e376d31380 175 pc.printf("Potmeter1, refpos: %f \r\n", Potmeter1);
GerhardBerman 7:2f74dfd1d411 176 pc.printf("Motor Axis Ref Position1: %f rad \r\n", referencePosition1);
GerhardBerman 7:2f74dfd1d411 177 return referencePosition1;
GerhardBerman 7:2f74dfd1d411 178 }
GerhardBerman 7:2f74dfd1d411 179 */
GerhardBerman 7:2f74dfd1d411 180
GerhardBerman 7:2f74dfd1d411 181 float FeedForwardControl1(float q1_dot){
GerhardBerman 7:2f74dfd1d411 182 //float Encoder1Position = counts1/resolution; //position in radians, encoder axis
GerhardBerman 7:2f74dfd1d411 183 //float Position1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 7:2f74dfd1d411 184
GerhardBerman 7:2f74dfd1d411 185 // linear feedback control
GerhardBerman 7:2f74dfd1d411 186 float error1 = q1_dot; //referencePosition1 - Position1; // proportional error in radians
GerhardBerman 7:2f74dfd1d411 187 float Kp = 1; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 188
GerhardBerman 7:2f74dfd1d411 189 float IntError1 = IntError1 + error1*t_sample; // integrated error in radians
GerhardBerman 7:2f74dfd1d411 190 //float maxKi = 0.2;
GerhardBerman 7:2f74dfd1d411 191 float Ki = 0.1; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 192
GerhardBerman 7:2f74dfd1d411 193 float DerivativeError1 = (error1_prev + error1)/t_sample; // derivative of error in radians
GerhardBerman 7:2f74dfd1d411 194 //float maxKd = 0.2;
GerhardBerman 7:2f74dfd1d411 195 float Kd = 0.0; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 196
GerhardBerman 7:2f74dfd1d411 197 //scope.set(0,referencePosition1);
GerhardBerman 7:2f74dfd1d411 198 //scope.set(1,Position1);
GerhardBerman 7:2f74dfd1d411 199 //scope.set(2,Ki);
GerhardBerman 7:2f74dfd1d411 200 //scope.send();
GerhardBerman 7:2f74dfd1d411 201
GerhardBerman 7:2f74dfd1d411 202 float motorValue1 = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd; //total controller output = motor input
GerhardBerman 7:2f74dfd1d411 203 //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
GerhardBerman 7:2f74dfd1d411 204 //pc.printf("Counts encoder1: %i rad \r\n", counts1);
GerhardBerman 7:2f74dfd1d411 205 //pc.printf("Kp: %f \r\n", Kp);
GerhardBerman 7:2f74dfd1d411 206 //pc.printf("MotorValue: %f \r\n", motorValue1);
GerhardBerman 7:2f74dfd1d411 207
GerhardBerman 7:2f74dfd1d411 208 error1_prev = error1;
GerhardBerman 7:2f74dfd1d411 209 return motorValue1;
GerhardBerman 3:8caef4872b0c 210 }
GerhardBerman 3:8caef4872b0c 211
GerhardBerman 7:2f74dfd1d411 212 float FeedForwardControl2(float q2_dot){
GerhardBerman 7:2f74dfd1d411 213 //float Encoder2Position = counts2/resolution; //position in radians, encoder axis
GerhardBerman 7:2f74dfd1d411 214 //float Position2 = Encoder2Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 6:3c4f3f2ce54f 215
GerhardBerman 7:2f74dfd1d411 216 // linear feedback control
GerhardBerman 7:2f74dfd1d411 217 float error2 = q2_dot; //referencePosition2 - Position2; // proportional error in radians
GerhardBerman 4:19e376d31380 218 float Kp = 1; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 219
GerhardBerman 7:2f74dfd1d411 220 float IntError2 = IntError2 + error2*t_sample; // integrated error in radians
GerhardBerman 7:2f74dfd1d411 221 //float maxKi = 0.2;
GerhardBerman 7:2f74dfd1d411 222 float Ki = 0.1; //potMeter2.read();
GerhardBerman 4:19e376d31380 223
GerhardBerman 7:2f74dfd1d411 224 float DerivativeError2 = (error2_prev + error2)/t_sample; // derivative of error in radians
GerhardBerman 7:2f74dfd1d411 225 //float maxKd = 0.2;
GerhardBerman 7:2f74dfd1d411 226 float Kd = 0.0; //potMeter2.read()*maxKd;
GerhardBerman 7:2f74dfd1d411 227
GerhardBerman 7:2f74dfd1d411 228 //scope.set(0,referencePosition1);
GerhardBerman 7:2f74dfd1d411 229 //scope.set(1,Position1);
GerhardBerman 7:2f74dfd1d411 230 //scope.set(2,Ki);
GerhardBerman 7:2f74dfd1d411 231 //scope.send();
GerhardBerman 4:19e376d31380 232
GerhardBerman 7:2f74dfd1d411 233 float motorValue2 = error2 * Kp + IntError2 * Ki + DerivativeError2 * Kd; //total controller output = motor input
GerhardBerman 7:2f74dfd1d411 234 //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
GerhardBerman 7:2f74dfd1d411 235 //pc.printf("Counts encoder1: %i rad \r\n", counts1);
GerhardBerman 7:2f74dfd1d411 236 //pc.printf("Kp: %f \r\n", Kp);
GerhardBerman 7:2f74dfd1d411 237 //pc.printf("MotorValue: %f \r\n", motorValue1);
GerhardBerman 6:3c4f3f2ce54f 238
GerhardBerman 7:2f74dfd1d411 239 error2_prev = error2;
GerhardBerman 7:2f74dfd1d411 240 return motorValue2;
GerhardBerman 3:8caef4872b0c 241 }
GerhardBerman 0:43160ef59f9f 242
GerhardBerman 7:2f74dfd1d411 243 void SetMotor1(float motorValue1)
GerhardBerman 3:8caef4872b0c 244 {
GerhardBerman 3:8caef4872b0c 245 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 3:8caef4872b0c 246 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 3:8caef4872b0c 247 // clockwise. motorValues outside range are truncated to
GerhardBerman 3:8caef4872b0c 248 // within range
GerhardBerman 7:2f74dfd1d411 249 if (motorValue1 >=0)
GerhardBerman 3:8caef4872b0c 250 {motor1DirectionPin=cw;
GerhardBerman 3:8caef4872b0c 251 led1=1;
GerhardBerman 3:8caef4872b0c 252 led2=0;
GerhardBerman 3:8caef4872b0c 253 }
GerhardBerman 3:8caef4872b0c 254 else {motor1DirectionPin=ccw;
GerhardBerman 3:8caef4872b0c 255 led1=0;
GerhardBerman 3:8caef4872b0c 256 led2=1;
GerhardBerman 3:8caef4872b0c 257 }
GerhardBerman 7:2f74dfd1d411 258 if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
GerhardBerman 7:2f74dfd1d411 259 else motor1MagnitudePin = fabs(motorValue1);
GerhardBerman 7:2f74dfd1d411 260 }
GerhardBerman 7:2f74dfd1d411 261
GerhardBerman 7:2f74dfd1d411 262 void SetMotor2(float motorValue2)
GerhardBerman 7:2f74dfd1d411 263 {
GerhardBerman 7:2f74dfd1d411 264 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 7:2f74dfd1d411 265 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 7:2f74dfd1d411 266 // clockwise. motorValues outside range are truncated to
GerhardBerman 7:2f74dfd1d411 267 // within range
GerhardBerman 7:2f74dfd1d411 268 if (motorValue2 >=0)
GerhardBerman 7:2f74dfd1d411 269 {motor2DirectionPin=cw;
GerhardBerman 7:2f74dfd1d411 270 led1=1;
GerhardBerman 7:2f74dfd1d411 271 led2=0;
GerhardBerman 7:2f74dfd1d411 272 }
GerhardBerman 7:2f74dfd1d411 273 else {motor2DirectionPin=ccw;
GerhardBerman 7:2f74dfd1d411 274 led1=0;
GerhardBerman 7:2f74dfd1d411 275 led2=1;
GerhardBerman 7:2f74dfd1d411 276 }
GerhardBerman 7:2f74dfd1d411 277 if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
GerhardBerman 7:2f74dfd1d411 278 else motor2MagnitudePin = fabs(motorValue2);
GerhardBerman 3:8caef4872b0c 279 }
GerhardBerman 3:8caef4872b0c 280
GerhardBerman 3:8caef4872b0c 281 void MeasureAndControl()
GerhardBerman 3:8caef4872b0c 282 {
GerhardBerman 3:8caef4872b0c 283 // This function measures the potmeter position, extracts a
GerhardBerman 3:8caef4872b0c 284 // reference position from it, and controls the motor with
GerhardBerman 3:8caef4872b0c 285 // a Feedback controller. Call this from a Ticker.
GerhardBerman 7:2f74dfd1d411 286 float referencePosition1 = GetReferenceKinematics1();
GerhardBerman 7:2f74dfd1d411 287 float referencePosition2 = GetReferenceKinematics2();
GerhardBerman 7:2f74dfd1d411 288 //float referencePosition1 = GetReferencePosition1();
GerhardBerman 7:2f74dfd1d411 289 //float referencePosition2 = GetReferencePosition2();
GerhardBerman 7:2f74dfd1d411 290 float motorValue1 = FeedForwardControl1(referencePosition1);
GerhardBerman 7:2f74dfd1d411 291 float motorValue2 = FeedForwardControl2(referencePosition2);
GerhardBerman 7:2f74dfd1d411 292 SetMotor1(motorValue1);
GerhardBerman 7:2f74dfd1d411 293 SetMotor2(motorValue2);
GerhardBerman 3:8caef4872b0c 294 }
GerhardBerman 3:8caef4872b0c 295
GerhardBerman 3:8caef4872b0c 296 void TimeTrackerF(){
GerhardBerman 3:8caef4872b0c 297 //wait(1);
GerhardBerman 3:8caef4872b0c 298 //float Potmeter1 = potMeter1.read();
GerhardBerman 7:2f74dfd1d411 299 //float referencePosition1 = GetReferencePosition();
GerhardBerman 7:2f74dfd1d411 300 //pc.printf("TTReference Position: %d rad \r\n", referencePosition1);
GerhardBerman 3:8caef4872b0c 301 //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
GerhardBerman 3:8caef4872b0c 302 //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
GerhardBerman 7:2f74dfd1d411 303 //pc.printf("TTCounts: %i \r\n", counts1);
GerhardBerman 3:8caef4872b0c 304 }
GerhardBerman 7:2f74dfd1d411 305
GerhardBerman 3:8caef4872b0c 306 /*
GerhardBerman 3:8caef4872b0c 307 void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts
GerhardBerman 3:8caef4872b0c 308 //double in=DerivativeCounts();
GerhardBerman 3:8caef4872b0c 309 bqcDerivativeCounts=bqc.step(DerivativeCounts);
GerhardBerman 3:8caef4872b0c 310 //return(bqcDerivativeCounts);
GerhardBerman 3:8caef4872b0c 311 }
GerhardBerman 6:3c4f3f2ce54f 312 */
GerhardBerman 6:3c4f3f2ce54f 313
GerhardBerman 0:43160ef59f9f 314 int main()
GerhardBerman 0:43160ef59f9f 315 {
GerhardBerman 0:43160ef59f9f 316 //Initialize
GerhardBerman 3:8caef4872b0c 317 led1=1;
GerhardBerman 3:8caef4872b0c 318 led2=1;
GerhardBerman 3:8caef4872b0c 319 pc.baud(115200);
GerhardBerman 3:8caef4872b0c 320 pc.printf("Test putty");
GerhardBerman 4:19e376d31380 321 MeasureTicker.attach(&MeasureTicker_act, 0.01f);
GerhardBerman 3:8caef4872b0c 322 bqc.add(&bq1).add(&bq2);
GerhardBerman 0:43160ef59f9f 323 QEI Encoder(D12, D13, NC, 32); // turns on encoder
GerhardBerman 0:43160ef59f9f 324
GerhardBerman 0:43160ef59f9f 325 while(1)
GerhardBerman 0:43160ef59f9f 326 {
GerhardBerman 3:8caef4872b0c 327 if (MeasureTicker_go){
GerhardBerman 3:8caef4872b0c 328 MeasureTicker_go=false;
GerhardBerman 3:8caef4872b0c 329 MeasureAndControl();
GerhardBerman 7:2f74dfd1d411 330 counts1 = Encoder1.getPulses(); // gives position of encoder
GerhardBerman 7:2f74dfd1d411 331 counts2 = Encoder2.getPulses(); // gives position of encoder
GerhardBerman 3:8caef4872b0c 332 pc.printf("Resolution: %f pulses/rad \r\n",resolution);
GerhardBerman 3:8caef4872b0c 333 }
GerhardBerman 6:3c4f3f2ce54f 334 /*
GerhardBerman 3:8caef4872b0c 335 if (BiQuadTicker_go){
GerhardBerman 3:8caef4872b0c 336 BiQuadTicker_go=false;
GerhardBerman 3:8caef4872b0c 337 BiQuadFilter();
GerhardBerman 3:8caef4872b0c 338 }
GerhardBerman 6:3c4f3f2ce54f 339 */
GerhardBerman 0:43160ef59f9f 340 }
GerhardBerman 0:43160ef59f9f 341 }