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@17:91d20d362e72, 2016-10-21 (annotated)

Committer:
GerhardBerman
Date:
Fri Oct 21 10:39:46 2016 +0000
Revision:
17:91d20d362e72
Parent:
16:9b7651fdf5a0 
Added things to consider before use in robot

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