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@12:05e5964b69a4, 2016-10-20 (annotated)

Committer:
GerhardBerman
Date:
Thu Oct 20 11:15:10 2016 +0000
Revision:
12:05e5964b69a4
Parent:
11:773b3532d50f
Child:
13:934af23bf416
Working with Putty, no spontaneous motor action!

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 9:e4c34f5665a0 11 DigitalIn encoder2A (D14);
GerhardBerman 9:e4c34f5665a0 12 DigitalIn encoder2B (D15);
GerhardBerman 0:43160ef59f9f 13 DigitalOut led1 (D11);
GerhardBerman 0:43160ef59f9f 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 7:2f74dfd1d411 43 int counts1 = 0;
GerhardBerman 7:2f74dfd1d411 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 4:19e376d31380 78 const int counts_per_revolution = 4200; //counts per motor axis revolution
GerhardBerman 4:19e376d31380 79 const int inverse_gear_ratio = 131;
GerhardBerman 4:19e376d31380 80 //const float motor_axial_resolution = counts_per_revolution/(2*PI);
GerhardBerman 4:19e376d31380 81 const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis
GerhardBerman 3:8caef4872b0c 82
GerhardBerman 12:05e5964b69a4 83 void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_dotOut, float &q2_dotOut){
GerhardBerman 7:2f74dfd1d411 84
GerhardBerman 7:2f74dfd1d411 85 //get joint positions q from encoder
GerhardBerman 7:2f74dfd1d411 86 float Encoder1Position = counts1/resolution; //position in radians, encoder axis
GerhardBerman 9:e4c34f5665a0 87 float Encoder2Position = counts2/resolution;
GerhardBerman 7:2f74dfd1d411 88
GerhardBerman 12:05e5964b69a4 89 float Motor1Position = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 12:05e5964b69a4 90 float Motor2Position = Encoder2Position*inverse_gear_ratio;
GerhardBerman 9:e4c34f5665a0 91
GerhardBerman 7:2f74dfd1d411 92 //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
GerhardBerman 11:773b3532d50f 93 float biceps1 = !button1.read();
GerhardBerman 11:773b3532d50f 94 float biceps2 = !button2.read();
GerhardBerman 8:935abf8ecc27 95 if (biceps1 > 0 && biceps2 > 0){
GerhardBerman 8:935abf8ecc27 96 //both arms activated: stamp moves down
GerhardBerman 10:45473f650198 97 led1 = 1;
GerhardBerman 10:45473f650198 98 led2 = 1;
GerhardBerman 8:935abf8ecc27 99 dx = 0;
GerhardBerman 12:05e5964b69a4 100 dy = 1; //dy_stampdown; //into stamping vertical position?? ~the stamp down action
GerhardBerman 12:05e5964b69a4 101 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 102 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 103
GerhardBerman 12:05e5964b69a4 104 /*
GerhardBerman 8:935abf8ecc27 105 wait(1);
GerhardBerman 8:935abf8ecc27 106 dy = -(dy_stampdown); //reset vertical position
GerhardBerman 12:05e5964b69a4 107 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 108 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 109 */
GerhardBerman 8:935abf8ecc27 110 }
GerhardBerman 8:935abf8ecc27 111 else if (biceps1 > 0 && biceps2 <= 0){
GerhardBerman 8:935abf8ecc27 112 //arm 1 activated, move left
GerhardBerman 10:45473f650198 113 led1 = 1;
GerhardBerman 10:45473f650198 114 led2 = 0;
GerhardBerman 12:05e5964b69a4 115 dx = 1; //-biceps1;
GerhardBerman 8:935abf8ecc27 116 dy = 0;
GerhardBerman 12:05e5964b69a4 117 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 118 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 119 }
GerhardBerman 8:935abf8ecc27 120 else if (biceps1 <= 0 && biceps2 > 0){
GerhardBerman 8:935abf8ecc27 121 //arm 1 activated, move left
GerhardBerman 10:45473f650198 122 led1 = 0;
GerhardBerman 10:45473f650198 123 led2 = 1;
GerhardBerman 12:05e5964b69a4 124 dx = 1; //biceps2;
GerhardBerman 8:935abf8ecc27 125 dy = 0;
GerhardBerman 12:05e5964b69a4 126 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 127 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 128
GerhardBerman 8:935abf8ecc27 129 }
GerhardBerman 8:935abf8ecc27 130 else{
GerhardBerman 10:45473f650198 131 led1 = 0;
GerhardBerman 10:45473f650198 132 led2 = 0;
GerhardBerman 8:935abf8ecc27 133 dx=0;
GerhardBerman 8:935abf8ecc27 134 dy=0;
GerhardBerman 12:05e5964b69a4 135 q1_dotOut = 0;
GerhardBerman 12:05e5964b69a4 136 q2_dotOut = 0;
GerhardBerman 8:935abf8ecc27 137 }
GerhardBerman 8:935abf8ecc27 138
GerhardBerman 7:2f74dfd1d411 139 //get joint angles change q_dot = Jpseudo * TwistEndEff (Matlab)
GerhardBerman 12:05e5964b69a4 140
GerhardBerman 7:2f74dfd1d411 141 //update joint angles
GerhardBerman 12:05e5964b69a4 142 q1Out = q1Out + q1_dotOut; //in radians
GerhardBerman 12:05e5964b69a4 143 q2Out = q2Out + q2_dotOut;
GerhardBerman 12:05e5964b69a4 144
GerhardBerman 12:05e5964b69a4 145 pc.baud(115200);
GerhardBerman 12:05e5964b69a4 146 pc.printf("dx: %f \r\n", dx);
GerhardBerman 12:05e5964b69a4 147 pc.printf("dy: %f \r\n", dy);
GerhardBerman 12:05e5964b69a4 148 pc.printf("Encoder1: %f \r\n", Encoder1Position);
GerhardBerman 12:05e5964b69a4 149 pc.printf("Motor1: %f \r\n", Motor1Position);
GerhardBerman 12:05e5964b69a4 150 pc.printf("q1: %f \r\n", q1Out);
GerhardBerman 12:05e5964b69a4 151 pc.printf("q1_dot: %f \r\n", q1_dotOut);
GerhardBerman 12:05e5964b69a4 152 pc.printf("Encoder2: %f \r\n", Encoder2Position);
GerhardBerman 12:05e5964b69a4 153 pc.printf("Motor2: %f \r\n", Motor2Position);
GerhardBerman 12:05e5964b69a4 154 pc.printf("q2: %f \r\n", q2Out);
GerhardBerman 12:05e5964b69a4 155 pc.printf("q2_dot: %f \r\n", q2_dotOut);
GerhardBerman 12:05e5964b69a4 156
GerhardBerman 9:e4c34f5665a0 157
GerhardBerman 7:2f74dfd1d411 158 }
GerhardBerman 7:2f74dfd1d411 159
GerhardBerman 9:e4c34f5665a0 160 void FeedForwardControl1(float q1_dot, float q2_dot, float &motorValue1Out, float &motorValue2Out){
GerhardBerman 7:2f74dfd1d411 161 //float Encoder1Position = counts1/resolution; //position in radians, encoder axis
GerhardBerman 7:2f74dfd1d411 162 //float Position1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
GerhardBerman 7:2f74dfd1d411 163
GerhardBerman 7:2f74dfd1d411 164 // linear feedback control
GerhardBerman 7:2f74dfd1d411 165 float error1 = q1_dot; //referencePosition1 - Position1; // proportional error in radians
GerhardBerman 9:e4c34f5665a0 166 float error2 = q2_dot; //referencePosition1 - Position1; // proportional error in radians
GerhardBerman 7:2f74dfd1d411 167 float Kp = 1; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 168
GerhardBerman 7:2f74dfd1d411 169 float IntError1 = IntError1 + error1*t_sample; // integrated error in radians
GerhardBerman 9:e4c34f5665a0 170 float IntError2 = IntError2 + error2*t_sample; // integrated error in radians
GerhardBerman 7:2f74dfd1d411 171 //float maxKi = 0.2;
GerhardBerman 7:2f74dfd1d411 172 float Ki = 0.1; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 173
GerhardBerman 7:2f74dfd1d411 174 float DerivativeError1 = (error1_prev + error1)/t_sample; // derivative of error in radians
GerhardBerman 9:e4c34f5665a0 175 float DerivativeError2 = (error2_prev + error2)/t_sample; // derivative of error in radians
GerhardBerman 7:2f74dfd1d411 176 //float maxKd = 0.2;
GerhardBerman 7:2f74dfd1d411 177 float Kd = 0.0; //potMeter2.read();
GerhardBerman 7:2f74dfd1d411 178
GerhardBerman 7:2f74dfd1d411 179 //scope.set(0,referencePosition1);
GerhardBerman 7:2f74dfd1d411 180 //scope.set(1,Position1);
GerhardBerman 7:2f74dfd1d411 181 //scope.set(2,Ki);
GerhardBerman 7:2f74dfd1d411 182 //scope.send();
GerhardBerman 7:2f74dfd1d411 183
GerhardBerman 9:e4c34f5665a0 184 motorValue1Out = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd; //total controller output = motor input
GerhardBerman 9:e4c34f5665a0 185 motorValue2Out = error2 * Kp + IntError2 * Ki + DerivativeError2 * Kd; //total controller output = motor input
GerhardBerman 7:2f74dfd1d411 186 //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
GerhardBerman 7:2f74dfd1d411 187 //pc.printf("Counts encoder1: %i rad \r\n", counts1);
GerhardBerman 7:2f74dfd1d411 188 //pc.printf("Kp: %f \r\n", Kp);
GerhardBerman 7:2f74dfd1d411 189 //pc.printf("MotorValue: %f \r\n", motorValue1);
GerhardBerman 7:2f74dfd1d411 190
GerhardBerman 12:05e5964b69a4 191 pc.printf("error1: %f \r\n", error1);
GerhardBerman 12:05e5964b69a4 192 pc.printf("IntError1: %f \r\n", IntError1);
GerhardBerman 12:05e5964b69a4 193 pc.printf("DerError1: %f \r\n", DerivativeError1);
GerhardBerman 12:05e5964b69a4 194 pc.printf("error2: %f \r\n", error2);
GerhardBerman 12:05e5964b69a4 195 pc.printf("IntError2: %f \r\n", IntError2);
GerhardBerman 12:05e5964b69a4 196 pc.printf("DerError2: %f \r\n", DerivativeError2);
GerhardBerman 12:05e5964b69a4 197
GerhardBerman 7:2f74dfd1d411 198 error1_prev = error1;
GerhardBerman 9:e4c34f5665a0 199 error2_prev = error1;
GerhardBerman 11:773b3532d50f 200 float biceps1 = !button1.read();
GerhardBerman 11:773b3532d50f 201 float biceps2 = !button2.read();
GerhardBerman 11:773b3532d50f 202
GerhardBerman 12:05e5964b69a4 203 /*
GerhardBerman 12:05e5964b69a4 204 scope.set(0,q1);
GerhardBerman 12:05e5964b69a4 205 scope.set(1,q2);
GerhardBerman 11:773b3532d50f 206 scope.set(2,biceps1);
GerhardBerman 11:773b3532d50f 207 scope.set(3,biceps2);
GerhardBerman 11:773b3532d50f 208 scope.send();
GerhardBerman 12:05e5964b69a4 209 */
GerhardBerman 3:8caef4872b0c 210 }
GerhardBerman 3:8caef4872b0c 211
GerhardBerman 9:e4c34f5665a0 212 void SetMotor1(float motorValue1, float motorValue2)
GerhardBerman 3:8caef4872b0c 213 {
GerhardBerman 3:8caef4872b0c 214 // Given -1<=motorValue<=1, this sets the PWM and direction
GerhardBerman 3:8caef4872b0c 215 // bits for motor 1. Positive value makes motor rotating
GerhardBerman 3:8caef4872b0c 216 // clockwise. motorValues outside range are truncated to
GerhardBerman 3:8caef4872b0c 217 // within range
GerhardBerman 9:e4c34f5665a0 218 //control motor 1
GerhardBerman 7:2f74dfd1d411 219 if (motorValue1 >=0)
GerhardBerman 3:8caef4872b0c 220 {motor1DirectionPin=cw;
GerhardBerman 9:e4c34f5665a0 221 //led1=1;
GerhardBerman 9:e4c34f5665a0 222 //led2=0;
GerhardBerman 3:8caef4872b0c 223 }
GerhardBerman 3:8caef4872b0c 224 else {motor1DirectionPin=ccw;
GerhardBerman 9:e4c34f5665a0 225 //led1=0;
GerhardBerman 9:e4c34f5665a0 226 //led2=1;
GerhardBerman 3:8caef4872b0c 227 }
GerhardBerman 7:2f74dfd1d411 228 if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
GerhardBerman 7:2f74dfd1d411 229 else motor1MagnitudePin = fabs(motorValue1);
GerhardBerman 9:e4c34f5665a0 230 //control motor 2
GerhardBerman 7:2f74dfd1d411 231 if (motorValue2 >=0)
GerhardBerman 7:2f74dfd1d411 232 {motor2DirectionPin=cw;
GerhardBerman 10:45473f650198 233 //led1=1;
GerhardBerman 10:45473f650198 234 //led2=0;
GerhardBerman 7:2f74dfd1d411 235 }
GerhardBerman 7:2f74dfd1d411 236 else {motor2DirectionPin=ccw;
GerhardBerman 10:45473f650198 237 //led1=0;
GerhardBerman 10:45473f650198 238 //led2=1;
GerhardBerman 7:2f74dfd1d411 239 }
GerhardBerman 7:2f74dfd1d411 240 if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
GerhardBerman 7:2f74dfd1d411 241 else motor2MagnitudePin = fabs(motorValue2);
GerhardBerman 3:8caef4872b0c 242 }
GerhardBerman 3:8caef4872b0c 243
GerhardBerman 3:8caef4872b0c 244 void MeasureAndControl()
GerhardBerman 3:8caef4872b0c 245 {
GerhardBerman 9:e4c34f5665a0 246 // This function measures the EMG of both arms, calculates via IK what
GerhardBerman 9:e4c34f5665a0 247 // the joint speeds should be, and controls the motor with
GerhardBerman 9:e4c34f5665a0 248 // a Feedforward controller. This is called from a Ticker.
GerhardBerman 12:05e5964b69a4 249 GetReferenceKinematics1(q1, q2, q1_dot, q2_dot);
GerhardBerman 9:e4c34f5665a0 250 FeedForwardControl1( q1_dot, q2_dot, motorValue1, motorValue2);
GerhardBerman 9:e4c34f5665a0 251 SetMotor1(motorValue1, motorValue2);
GerhardBerman 3:8caef4872b0c 252 }
GerhardBerman 3:8caef4872b0c 253
GerhardBerman 3:8caef4872b0c 254 void TimeTrackerF(){
GerhardBerman 3:8caef4872b0c 255 //wait(1);
GerhardBerman 3:8caef4872b0c 256 //float Potmeter1 = potMeter1.read();
GerhardBerman 7:2f74dfd1d411 257 //float referencePosition1 = GetReferencePosition();
GerhardBerman 7:2f74dfd1d411 258 //pc.printf("TTReference Position: %d rad \r\n", referencePosition1);
GerhardBerman 3:8caef4872b0c 259 //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
GerhardBerman 3:8caef4872b0c 260 //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
GerhardBerman 7:2f74dfd1d411 261 //pc.printf("TTCounts: %i \r\n", counts1);
GerhardBerman 3:8caef4872b0c 262 }
GerhardBerman 7:2f74dfd1d411 263
GerhardBerman 3:8caef4872b0c 264 /*
GerhardBerman 3:8caef4872b0c 265 void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts
GerhardBerman 3:8caef4872b0c 266 //double in=DerivativeCounts();
GerhardBerman 3:8caef4872b0c 267 bqcDerivativeCounts=bqc.step(DerivativeCounts);
GerhardBerman 3:8caef4872b0c 268 //return(bqcDerivativeCounts);
GerhardBerman 3:8caef4872b0c 269 }
GerhardBerman 9:e4c34f5665a0 270 */
GerhardBerman 6:3c4f3f2ce54f 271
GerhardBerman 0:43160ef59f9f 272 int main()
GerhardBerman 0:43160ef59f9f 273 {
GerhardBerman 0:43160ef59f9f 274 //Initialize
GerhardBerman 10:45473f650198 275 int led1val = led1.read();
GerhardBerman 10:45473f650198 276 int led2val = led2.read();
GerhardBerman 9:e4c34f5665a0 277 pc.baud(115200);
GerhardBerman 12:05e5964b69a4 278 pc.printf("Test putty IK");
GerhardBerman 9:e4c34f5665a0 279 MeasureTicker.attach(&MeasureTicker_act, 1.0f);
GerhardBerman 10:45473f650198 280 bqc.add(&bq1).add(&bq2);
GerhardBerman 10:45473f650198 281
GerhardBerman 0:43160ef59f9f 282 while(1)
GerhardBerman 0:43160ef59f9f 283 {
GerhardBerman 3:8caef4872b0c 284 if (MeasureTicker_go){
GerhardBerman 3:8caef4872b0c 285 MeasureTicker_go=false;
GerhardBerman 3:8caef4872b0c 286 MeasureAndControl();
GerhardBerman 7:2f74dfd1d411 287 counts1 = Encoder1.getPulses(); // gives position of encoder
GerhardBerman 7:2f74dfd1d411 288 counts2 = Encoder2.getPulses(); // gives position of encoder
GerhardBerman 3:8caef4872b0c 289 }
GerhardBerman 10:45473f650198 290 /*
GerhardBerman 3:8caef4872b0c 291 if (BiQuadTicker_go){
GerhardBerman 3:8caef4872b0c 292 BiQuadTicker_go=false;
GerhardBerman 3:8caef4872b0c 293 BiQuadFilter();
GerhardBerman 3:8caef4872b0c 294 }
GerhardBerman 10:45473f650198 295 */
GerhardBerman 0:43160ef59f9f 296 }
GerhardBerman 0:43160ef59f9f 297 }