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

Committer:
GerhardBerman
Date:
Thu Oct 20 12:31:42 2016 +0000
Revision:
14:725a608b6709
Parent:
13:934af23bf416
Child:
15:9061cf7db23e
Encoder pins switched, cw/ccw problem changed. Not yet tested

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