Gerhard Berman
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
Diff: main.cpp
- Revision:
- 15:9061cf7db23e
- Parent:
- 14:725a608b6709
- Child:
- 16:9b7651fdf5a0
--- a/main.cpp Thu Oct 20 12:31:42 2016 +0000
+++ b/main.cpp Thu Oct 20 13:11:09 2016 +0000
@@ -40,8 +40,8 @@
float motorValue2 = 0.0;
//set constant or variable values
-//int counts1 = 0;
-//int counts2 = 0;
+int counts1 = 0;
+int counts2 = 0;
int counts1Prev = 0;
int counts2Prev = 0;
double DerivativeCounts;
@@ -74,9 +74,7 @@
//define encoder counts and degrees
QEI Encoder1(D12, D13, NC, 32); // turns on encoder
-QEI Encoder2(D14, D15, NC, 32); // turns on encoder
- int counts1 = Encoder1.getPulses(); // gives position of encoder
- int counts2 = Encoder2.getPulses(); // gives position of encoder
+QEI Encoder2(D10, D11, NC, 32); // turns on encoder
const int counts_per_revolution = 4200; //counts per motor axis revolution
const int inverse_gear_ratio = 131;
@@ -97,8 +95,8 @@
float biceps2 = !button2.read();
if (biceps1 > 0 && biceps2 > 0){
//both arms activated: stamp moves down
- led1 = 1;
- led2 = 1;
+ //led1 = 1;
+ //led2 = 1;
dx = 0;
dy = 1; //dy_stampdown; //into stamping vertical position?? ~the stamp down action
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)));
@@ -113,8 +111,8 @@
}
else if (biceps1 > 0 && biceps2 <= 0){
//arm 1 activated, move left
- led1 = 1;
- led2 = 0;
+ //led1 = 1;
+ //led2 = 0;
dx = 1; //-biceps1;
dy = 0;
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)));
@@ -122,8 +120,8 @@
}
else if (biceps1 <= 0 && biceps2 > 0){
//arm 1 activated, move left
- led1 = 0;
- led2 = 1;
+ //led1 = 0;
+ //led2 = 1;
dx = 1; //biceps2;
dy = 0;
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)));
@@ -131,8 +129,8 @@
}
else{
- led1 = 0;
- led2 = 0;
+ //led1 = 0;
+ //led2 = 0;
dx=0;
dy=0;
q1_dotOut = 0;
@@ -220,24 +218,26 @@
// clockwise. motorValues outside range are truncated to
// within range
//control motor 1
- if (motorValue1 >=0)
- {motor1DirectionPin=cw;
+ if (motorValue1 >=0) //clockwise rotation
+ {motor1DirectionPin=ccw; //inverted due to opposite (to other motor) build-up in tower
//led1=1;
//led2=0;
}
- else {motor1DirectionPin=ccw;
+ else //counterclockwise rotation
+ {motor1DirectionPin=cw; //inverted due to opposite (to other motor) build-up in tower
//led1=0;
//led2=1;
}
if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
else motor1MagnitudePin = fabs(motorValue1);
//control motor 2
- if (motorValue2 >=0)
+ if (motorValue2 >=0) //clockwise rotation
{motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
//led1=1;
//led2=0;
}
- else {motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted)
+ else //counterclockwise rotation
+ {motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted)
//led1=0;
//led2=1;
}
@@ -276,8 +276,8 @@
int main()
{
//Initialize
- int led1val = led1.read();
- int led2val = led2.read();
+ //int led1val = led1.read();
+ //int led2val = led2.read();
pc.baud(115200);
pc.printf("Test putty IK");
MeasureTicker.attach(&MeasureTicker_act, 1.0f);
@@ -288,8 +288,8 @@
if (MeasureTicker_go){
MeasureTicker_go=false;
MeasureAndControl();
- //counts1 = Encoder1.getPulses(); // gives position of encoder
- //counts2 = Encoder2.getPulses(); // gives position of encoder
+ counts1 = Encoder1.getPulses(); // gives position of encoder
+ counts2 = Encoder2.getPulses(); // gives position of encoder
}
/*
if (BiQuadTicker_go){