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@16:9b7651fdf5a0, 2016-10-20 (annotated)

Committer:
GerhardBerman
Date:
Thu Oct 20 13:14:03 2016 +0000
Revision:
16:9b7651fdf5a0
Parent:
15:9061cf7db23e
Child:
17:91d20d362e72
All working, motorValues scaled down to 1/10 for first testing.

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 15:9061cf7db23e 43 int counts1 = 0;
GerhardBerman 15:9061cf7db23e 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 15:9061cf7db23e 77 QEI Encoder2(D10, D11, NC, 32); // turns on encoder
GerhardBerman 14:725a608b6709 78
GerhardBerman 4:19e376d31380 79 const int counts_per_revolution = 4200; //counts per motor axis revolution
GerhardBerman 4:19e376d31380 80 const int inverse_gear_ratio = 131;
GerhardBerman 4:19e376d31380 81 //const float motor_axial_resolution = counts_per_revolution/(2*PI);
GerhardBerman 4:19e376d31380 82 const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis
GerhardBerman 3:8caef4872b0c 83
GerhardBerman 12:05e5964b69a4 84 void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_dotOut, float &q2_dotOut){
GerhardBerman 7:2f74dfd1d411 85
GerhardBerman 7:2f74dfd1d411 86 //get joint positions q from encoder
GerhardBerman 7:2f74dfd1d411 87 float Encoder1Position = counts1/resolution; //position in radians, encoder axis
GerhardBerman 9:e4c34f5665a0 88 float Encoder2Position = counts2/resolution;
GerhardBerman 7:2f74dfd1d411 89
GerhardBerman 12:05e5964b69a4 90 float Motor1Position = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 12:05e5964b69a4 91 float Motor2Position = Encoder2Position*inverse_gear_ratio;
GerhardBerman 9:e4c34f5665a0 92
GerhardBerman 7:2f74dfd1d411 93 //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
GerhardBerman 11:773b3532d50f 94 float biceps1 = !button1.read();
GerhardBerman 11:773b3532d50f 95 float biceps2 = !button2.read();
GerhardBerman 8:935abf8ecc27 96 if (biceps1 > 0 && biceps2 > 0){
GerhardBerman 8:935abf8ecc27 97 //both arms activated: stamp moves down
GerhardBerman 15:9061cf7db23e 98 //led1 = 1;
GerhardBerman 15:9061cf7db23e 99 //led2 = 1;
GerhardBerman 8:935abf8ecc27 100 dx = 0;
GerhardBerman 12:05e5964b69a4 101 dy = 1; //dy_stampdown; //into stamping vertical position?? ~the stamp down action
GerhardBerman 12:05e5964b69a4 102 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 103 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 104
GerhardBerman 12:05e5964b69a4 105 /*
GerhardBerman 8:935abf8ecc27 106 wait(1);
GerhardBerman 8:935abf8ecc27 107 dy = -(dy_stampdown); //reset vertical position
GerhardBerman 12:05e5964b69a4 108 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 109 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 110 */
GerhardBerman 8:935abf8ecc27 111 }
GerhardBerman 8:935abf8ecc27 112 else if (biceps1 > 0 && biceps2 <= 0){
GerhardBerman 8:935abf8ecc27 113 //arm 1 activated, move left
GerhardBerman 15:9061cf7db23e 114 //led1 = 1;
GerhardBerman 15:9061cf7db23e 115 //led2 = 0;
GerhardBerman 12:05e5964b69a4 116 dx = 1; //-biceps1;
GerhardBerman 8:935abf8ecc27 117 dy = 0;
GerhardBerman 12:05e5964b69a4 118 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 119 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 120 }
GerhardBerman 8:935abf8ecc27 121 else if (biceps1 <= 0 && biceps2 > 0){
GerhardBerman 8:935abf8ecc27 122 //arm 1 activated, move left
GerhardBerman 15:9061cf7db23e 123 //led1 = 0;
GerhardBerman 15:9061cf7db23e 124 //led2 = 1;
GerhardBerman 12:05e5964b69a4 125 dx = 1; //biceps2;
GerhardBerman 8:935abf8ecc27 126 dy = 0;
GerhardBerman 12:05e5964b69a4 127 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 128 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 129
GerhardBerman 8:935abf8ecc27 130 }
GerhardBerman 8:935abf8ecc27 131 else{
GerhardBerman 15:9061cf7db23e 132 //led1 = 0;
GerhardBerman 15:9061cf7db23e 133 //led2 = 0;
GerhardBerman 8:935abf8ecc27 134 dx=0;
GerhardBerman 8:935abf8ecc27 135 dy=0;
GerhardBerman 12:05e5964b69a4 136 q1_dotOut = 0;
GerhardBerman 12:05e5964b69a4 137 q2_dotOut = 0;
GerhardBerman 8:935abf8ecc27 138 }
GerhardBerman 8:935abf8ecc27 139
GerhardBerman 7:2f74dfd1d411 140 //get joint angles change q_dot = Jpseudo * TwistEndEff (Matlab)
GerhardBerman 12:05e5964b69a4 141
GerhardBerman 7:2f74dfd1d411 142 //update joint angles
GerhardBerman 12:05e5964b69a4 143 q1Out = q1Out + q1_dotOut; //in radians
GerhardBerman 12:05e5964b69a4 144 q2Out = q2Out + q2_dotOut;
GerhardBerman 12:05e5964b69a4 145
GerhardBerman 12:05e5964b69a4 146 pc.baud(115200);
GerhardBerman 12:05e5964b69a4 147 pc.printf("dx: %f \r\n", dx);
GerhardBerman 12:05e5964b69a4 148 pc.printf("dy: %f \r\n", dy);
GerhardBerman 12:05e5964b69a4 149 pc.printf("q1: %f \r\n", q1Out);
GerhardBerman 12:05e5964b69a4 150 pc.printf("q1_dot: %f \r\n", q1_dotOut);
GerhardBerman 12:05e5964b69a4 151 pc.printf("q2: %f \r\n", q2Out);
GerhardBerman 12:05e5964b69a4 152 pc.printf("q2_dot: %f \r\n", q2_dotOut);
GerhardBerman 12:05e5964b69a4 153
GerhardBerman 14:725a608b6709 154 pc.printf("Counts1: %f \r\n", counts1);
GerhardBerman 14:725a608b6709 155 pc.printf("Encoder1: %f \r\n", Encoder1Position);
GerhardBerman 14:725a608b6709 156 pc.printf("Motor1: %f \r\n", Motor1Position);
GerhardBerman 14:725a608b6709 157 pc.printf("Counts2: %f \r\n", counts2);
GerhardBerman 14:725a608b6709 158 pc.printf("Encoder2: %f \r\n", Encoder2Position);
GerhardBerman 14:725a608b6709 159 pc.printf("Motor2: %f \r\n", Motor2Position);
GerhardBerman 7:2f74dfd1d411 160 }
GerhardBerman 7:2f74dfd1d411 161
GerhardBerman 9:e4c34f5665a0 162 void FeedForwardControl1(float q1_dot, float q2_dot, float &motorValue1Out, float &motorValue2Out){
GerhardBerman 7:2f74dfd1d411 163 //float Encoder1Position = counts1/resolution; //position in radians, encoder axis
GerhardBerman 7:2f74dfd1d411 164 //float Position1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 7:2f74dfd1d411 165
GerhardBerman 7:2f74dfd1d411 166 // linear feedback control
GerhardBerman 7:2f74dfd1d411 167 float error1 = q1_dot; //referencePosition1 - Position1; // proportional error in radians
GerhardBerman 9:e4c34f5665a0 168 float error2 = q2_dot; //referencePosition1 - Position1; // proportional error in radians
GerhardBerman 7:2f74dfd1d411 169 float Kp = 1; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 170
GerhardBerman 7:2f74dfd1d411 171 float IntError1 = IntError1 + error1*t_sample; // integrated error in radians
GerhardBerman 9:e4c34f5665a0 172 float IntError2 = IntError2 + error2*t_sample; // integrated error in radians
GerhardBerman 7:2f74dfd1d411 173 //float maxKi = 0.2;
GerhardBerman 7:2f74dfd1d411 174 float Ki = 0.1; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 175
GerhardBerman 7:2f74dfd1d411 176 float DerivativeError1 = (error1_prev + error1)/t_sample; // derivative of error in radians
GerhardBerman 9:e4c34f5665a0 177 float DerivativeError2 = (error2_prev + error2)/t_sample; // derivative of error in radians
GerhardBerman 7:2f74dfd1d411 178 //float maxKd = 0.2;
GerhardBerman 7:2f74dfd1d411 179 float Kd = 0.0; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 180
GerhardBerman 7:2f74dfd1d411 181 //scope.set(0,referencePosition1);
GerhardBerman 7:2f74dfd1d411 182 //scope.set(1,Position1);
GerhardBerman 7:2f74dfd1d411 183 //scope.set(2,Ki);
GerhardBerman 7:2f74dfd1d411 184 //scope.send();
GerhardBerman 7:2f74dfd1d411 185
GerhardBerman 9:e4c34f5665a0 186 motorValue1Out = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd; //total controller output = motor input
GerhardBerman 9:e4c34f5665a0 187 motorValue2Out = error2 * Kp + IntError2 * Ki + DerivativeError2 * Kd; //total controller output = motor input
GerhardBerman 7:2f74dfd1d411 188 //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
GerhardBerman 7:2f74dfd1d411 189 //pc.printf("Counts encoder1: %i rad \r\n", counts1);
GerhardBerman 7:2f74dfd1d411 190 //pc.printf("Kp: %f \r\n", Kp);
GerhardBerman 7:2f74dfd1d411 191 //pc.printf("MotorValue: %f \r\n", motorValue1);
GerhardBerman 7:2f74dfd1d411 192
GerhardBerman 12:05e5964b69a4 193 pc.printf("error1: %f \r\n", error1);
GerhardBerman 12:05e5964b69a4 194 pc.printf("IntError1: %f \r\n", IntError1);
GerhardBerman 12:05e5964b69a4 195 pc.printf("DerError1: %f \r\n", DerivativeError1);
GerhardBerman 12:05e5964b69a4 196 pc.printf("error2: %f \r\n", error2);
GerhardBerman 12:05e5964b69a4 197 pc.printf("IntError2: %f \r\n", IntError2);
GerhardBerman 12:05e5964b69a4 198 pc.printf("DerError2: %f \r\n", DerivativeError2);
GerhardBerman 12:05e5964b69a4 199
GerhardBerman 7:2f74dfd1d411 200 error1_prev = error1;
GerhardBerman 9:e4c34f5665a0 201 error2_prev = error1;
GerhardBerman 11:773b3532d50f 202 float biceps1 = !button1.read();
GerhardBerman 11:773b3532d50f 203 float biceps2 = !button2.read();
GerhardBerman 11:773b3532d50f 204
GerhardBerman 12:05e5964b69a4 205 /*
GerhardBerman 12:05e5964b69a4 206 scope.set(0,q1);
GerhardBerman 12:05e5964b69a4 207 scope.set(1,q2);
GerhardBerman 11:773b3532d50f 208 scope.set(2,biceps1);
GerhardBerman 11:773b3532d50f 209 scope.set(3,biceps2);
GerhardBerman 11:773b3532d50f 210 scope.send();
GerhardBerman 12:05e5964b69a4 211 */
GerhardBerman 3:8caef4872b0c 212 }
GerhardBerman 3:8caef4872b0c 213
GerhardBerman 9:e4c34f5665a0 214 void SetMotor1(float motorValue1, float motorValue2)
GerhardBerman 3:8caef4872b0c 215 {
GerhardBerman 3:8caef4872b0c 216 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 3:8caef4872b0c 217 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 3:8caef4872b0c 218 // clockwise. motorValues outside range are truncated to
GerhardBerman 3:8caef4872b0c 219 // within range
GerhardBerman 9:e4c34f5665a0 220 //control motor 1
GerhardBerman 15:9061cf7db23e 221 if (motorValue1 >=0) //clockwise rotation
GerhardBerman 15:9061cf7db23e 222 {motor1DirectionPin=ccw; //inverted due to opposite (to other motor) build-up in tower
GerhardBerman 9:e4c34f5665a0 223 //led1=1;
GerhardBerman 9:e4c34f5665a0 224 //led2=0;
GerhardBerman 3:8caef4872b0c 225 }
GerhardBerman 15:9061cf7db23e 226 else //counterclockwise rotation
GerhardBerman 15:9061cf7db23e 227 {motor1DirectionPin=cw; //inverted due to opposite (to other motor) build-up in tower
GerhardBerman 9:e4c34f5665a0 228 //led1=0;
GerhardBerman 9:e4c34f5665a0 229 //led2=1;
GerhardBerman 3:8caef4872b0c 230 }
GerhardBerman 7:2f74dfd1d411 231 if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
GerhardBerman 16:9b7651fdf5a0 232 else motor1MagnitudePin = 0.1*fabs(motorValue1); //fabs(motorValue1);
GerhardBerman 9:e4c34f5665a0 233 //control motor 2
GerhardBerman 15:9061cf7db23e 234 if (motorValue2 >=0) //clockwise rotation
GerhardBerman 14:725a608b6709 235 {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
GerhardBerman 10:45473f650198 236 //led1=1;
GerhardBerman 10:45473f650198 237 //led2=0;
GerhardBerman 7:2f74dfd1d411 238 }
GerhardBerman 15:9061cf7db23e 239 else //counterclockwise rotation
GerhardBerman 15:9061cf7db23e 240 {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 16:9b7651fdf5a0 245 else motor2MagnitudePin = 0.1*fabs(motorValue2); //fabs(motorValue1);
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 15:9061cf7db23e 279 //int led1val = led1.read();
GerhardBerman 15:9061cf7db23e 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 15:9061cf7db23e 291 counts1 = Encoder1.getPulses(); // gives position of encoder
GerhardBerman 15:9061cf7db23e 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 }