Elfi Hofmeijer
With added boundaries, without errors, not yet tested
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
prog_forwardkin_feedback_copy
by 
Gerhard Berman
Diff: main.cpp
- Revision:
- 9:e4c34f5665a0
- Parent:
- 8:935abf8ecc27
- Child:
- 10:45473f650198
--- a/main.cpp Wed Oct 19 14:16:10 2016 +0000
+++ b/main.cpp Wed Oct 19 15:38:44 2016 +0000
@@ -8,6 +8,8 @@
//set pins
DigitalIn encoder1A (D13); //Channel A van Encoder 1
DigitalIn encoder1B (D12); //Channel B van Encoder 1
+DigitalIn encoder2A (D14);
+DigitalIn encoder2B (D15);
DigitalOut led1 (D11);
DigitalOut led2 (D10);
AnalogIn potMeter1(A2);
@@ -16,7 +18,7 @@
PwmOut motor1MagnitudePin(D6);
DigitalOut motor2DirectionPin(D4);
PwmOut motor2MagnitudePin(D5);
-DigitalIn button1(D8);
+DigitalIn button1(D3);
DigitalIn button2(D9);
//library settings
@@ -31,9 +33,12 @@
float IntError2 = 0;
float q1 = 0;
float q2 = 0;
-float q1_dot;
-float q2_dot;
-
+//set initial conditions for function references
+ float q1_dot = 0.0;
+ float q2_dot = 0.0;
+ float motorValue1 = 0.0;
+ float motorValue2 = 0.0;
+
//set constant or variable values
int counts1 = 0;
int counts2 = 0;
@@ -75,24 +80,22 @@
//const float motor_axial_resolution = counts_per_revolution/(2*PI);
const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis
-float GetReferenceKinematics1(){
+void GetReferenceKinematics1(float &q1_dotOut, float &q2_dotOut){
//get joint positions q from encoder
float Encoder1Position = counts1/resolution; //position in radians, encoder axis
- float q1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
-
- //float Encoder2Position = counts2/resolution; //position in radians, encoder axis
- //float q2 = Encoder2Position*inverse_gear_ratio; //position in radians, motor axis
+ float Encoder2Position = counts2/resolution;
- //NOTNECESSARY calculate end effector position with Brockett
-
- //NOTNECESSARY get desired position Pe* from EMG(?)
-
+ float q1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
+ float q2 = Encoder2Position*inverse_gear_ratio;
+
//get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
float biceps1 = button1.read();
float biceps2 = button2.read();
if (biceps1 > 0 && biceps2 > 0){
//both arms activated: stamp moves down
+ //led1 = 1;
+ //led2 = 1;
dx = 0;
dy = dy_stampdown; //into stamping vertical position?? ~the stamp down action
wait(1);
@@ -100,97 +103,51 @@
}
else if (biceps1 > 0 && biceps2 <= 0){
//arm 1 activated, move left
+ //led1 = 1;
+ //led2 = 0;
dx = -biceps1;
dy = 0;
}
else if (biceps1 <= 0 && biceps2 > 0){
//arm 1 activated, move left
+ //led1 = 0;
+ //led2 = 1;
dx = biceps2;
dy = 0;
}
else{
+ //led1 = 0;
+ //led2 = 0;
dx=0;
dy=0;
}
//get joint angles change q_dot = Jpseudo * TwistEndEff (Matlab)
- float q1_dot = 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)));
+ 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)));
+ 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)));
//update joint angles
- q1 = q1 + q1_dot;
- return q1_dot;
+ q1 = q1 + q1_dotOut;
+ q2 = q2 + q2_dotOut;
+
}
-float GetReferenceKinematics2(){
-
- //get joint positions q from encoder
- float Encoder1Position = counts1/resolution; //position in radians, encoder axis
- float q1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
-
- float Encoder2Position = counts2/resolution; //position in radians, encoder axis
- float q2 = Encoder2Position*inverse_gear_ratio; //position in radians, motor axis
-
- //NOTNECESSARY calculate end effector position with Brockett
-
- //NOTNECESSARY get desired position Pe* from EMG(?)
-
- //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
- float biceps1 = button1.read();
- float biceps2 = button2.read();
- while (biceps1 > 0){
- if (biceps2 > 0){ //both arms activated: stamp moves down
- dx = 0;
- dy = dy_stampdown; //into stamping vertical position?? ~the stamp down action
- wait(1);
- dy = -(dy_stampdown); //reset vertical position
- }
- else{ //left arm activated
- dx = biceps1;
- dy = 0;
- }
- while (biceps2 > 0){
- if (biceps1 <= 0){ //right arm activated
- dx = -biceps2;
- dy = 0;
- }
- }
-
- //get joint angles change q_dot = Jpseudo * TwistEndEff; (Matlab)
- float q2_dot = 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)));
-
- //update joint angles
- q2 = q2 + q2_dot;
- }
- return q2_dot;
- }
-
-/*
-float GetReferencePosition(){
- // Returns reference position in rad.
- // Positive value means clockwise rotation.
- const float maxPosition = 2*PI; //6.283185307179586; // in radians
- float Potmeter1 = potMeter1.read();
- float referencePosition1 = Potmeter1 * maxPosition; //Potmeter1 * maxPosition; //refpos in radians
- pc.printf("Max Position: %f rad \r\n", maxPosition);
- pc.printf("Potmeter1, refpos: %f \r\n", Potmeter1);
- pc.printf("Motor Axis Ref Position1: %f rad \r\n", referencePosition1);
- return referencePosition1;
-}
-*/
-
-float FeedForwardControl1(float q1_dot){
+void FeedForwardControl1(float q1_dot, float q2_dot, float &motorValue1Out, float &motorValue2Out){
//float Encoder1Position = counts1/resolution; //position in radians, encoder axis
//float Position1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis
// linear feedback control
float error1 = q1_dot; //referencePosition1 - Position1; // proportional error in radians
+ float error2 = q2_dot; //referencePosition1 - Position1; // proportional error in radians
float Kp = 1; //potMeter2.read();
float IntError1 = IntError1 + error1*t_sample; // integrated error in radians
+ float IntError2 = IntError2 + error2*t_sample; // integrated error in radians
//float maxKi = 0.2;
float Ki = 0.1; //potMeter2.read();
float DerivativeError1 = (error1_prev + error1)/t_sample; // derivative of error in radians
+ float DerivativeError2 = (error2_prev + error2)/t_sample; // derivative of error in radians
//float maxKd = 0.2;
float Kd = 0.0; //potMeter2.read();
@@ -199,72 +156,36 @@
//scope.set(2,Ki);
//scope.send();
- float motorValue1 = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd; //total controller output = motor input
+ motorValue1Out = error1 * Kp + IntError1 * Ki + DerivativeError1 * Kd; //total controller output = motor input
+ motorValue2Out = error2 * Kp + IntError2 * Ki + DerivativeError2 * Kd; //total controller output = motor input
//pc.printf("Motor Axis Position: %f rad \r\n", Position1);
//pc.printf("Counts encoder1: %i rad \r\n", counts1);
//pc.printf("Kp: %f \r\n", Kp);
//pc.printf("MotorValue: %f \r\n", motorValue1);
error1_prev = error1;
- return motorValue1;
+ error2_prev = error1;
}
-float FeedForwardControl2(float q2_dot){
- //float Encoder2Position = counts2/resolution; //position in radians, encoder axis
- //float Position2 = Encoder2Position*inverse_gear_ratio; //position in radians, motor axis
-
- // linear feedback control
- float error2 = q2_dot; //referencePosition2 - Position2; // proportional error in radians
- float Kp = 1; //potMeter2.read();
-
- float IntError2 = IntError2 + error2*t_sample; // integrated error in radians
- //float maxKi = 0.2;
- float Ki = 0.1; //potMeter2.read();
-
- float DerivativeError2 = (error2_prev + error2)/t_sample; // derivative of error in radians
- //float maxKd = 0.2;
- float Kd = 0.0; //potMeter2.read()*maxKd;
-
- //scope.set(0,referencePosition1);
- //scope.set(1,Position1);
- //scope.set(2,Ki);
- //scope.send();
-
- float motorValue2 = error2 * Kp + IntError2 * Ki + DerivativeError2 * Kd; //total controller output = motor input
- //pc.printf("Motor Axis Position: %f rad \r\n", Position1);
- //pc.printf("Counts encoder1: %i rad \r\n", counts1);
- //pc.printf("Kp: %f \r\n", Kp);
- //pc.printf("MotorValue: %f \r\n", motorValue1);
-
- error2_prev = error2;
- return motorValue2;
-}
-
-void SetMotor1(float motorValue1)
+void SetMotor1(float motorValue1, float motorValue2)
{
// Given -1<=motorValue<=1, this sets the PWM and direction
// bits for motor 1. Positive value makes motor rotating
// clockwise. motorValues outside range are truncated to
// within range
+ //control motor 1
if (motorValue1 >=0)
{motor1DirectionPin=cw;
- led1=1;
- led2=0;
+ //led1=1;
+ //led2=0;
}
else {motor1DirectionPin=ccw;
- led1=0;
- led2=1;
+ //led1=0;
+ //led2=1;
}
if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
else motor1MagnitudePin = fabs(motorValue1);
-}
-
-void SetMotor2(float motorValue2)
-{
- // Given -1<=motorValue<=1, this sets the PWM and direction
- // bits for motor 1. Positive value makes motor rotating
- // clockwise. motorValues outside range are truncated to
- // within range
+ //control motor 2
if (motorValue2 >=0)
{motor2DirectionPin=cw;
led1=1;
@@ -280,17 +201,12 @@
void MeasureAndControl()
{
- // This function measures the potmeter position, extracts a
- // reference position from it, and controls the motor with
- // a Feedback controller. Call this from a Ticker.
- float referencePosition1 = GetReferenceKinematics1();
- float referencePosition2 = GetReferenceKinematics2();
- //float referencePosition1 = GetReferencePosition1();
- //float referencePosition2 = GetReferencePosition2();
- float motorValue1 = FeedForwardControl1(referencePosition1);
- float motorValue2 = FeedForwardControl2(referencePosition2);
- SetMotor1(motorValue1);
- SetMotor2(motorValue2);
+ // This function measures the EMG of both arms, calculates via IK what
+ // the joint speeds should be, and controls the motor with
+ // a Feedforward controller. This is called from a Ticker.
+ GetReferenceKinematics1(q1_dot, q2_dot);
+ FeedForwardControl1( q1_dot, q2_dot, motorValue1, motorValue2);
+ SetMotor1(motorValue1, motorValue2);
}
void TimeTrackerF(){
@@ -309,19 +225,24 @@
bqcDerivativeCounts=bqc.step(DerivativeCounts);
//return(bqcDerivativeCounts);
}
- */
+*/
int main()
{
//Initialize
- led1=1;
- led2=1;
- pc.baud(115200);
+ pc.baud(115200);
pc.printf("Test putty");
- MeasureTicker.attach(&MeasureTicker_act, 0.01f);
- bqc.add(&bq1).add(&bq2);
- QEI Encoder(D12, D13, NC, 32); // turns on encoder
-
+ led1=1;
+ led1 = 0;
+ led2 = 0;
+ wait(2.0);
+ led1 = 1;
+ led2 = 1;
+ wait(2.0);
+ MeasureTicker.attach(&MeasureTicker_act, 1.0f);
+ //bqc.add(&bq1).add(&bq2);
+ //QEI Encoder1(D12, D13, NC, 32); // turns on encoder
+ /*
while(1)
{
if (MeasureTicker_go){
@@ -331,11 +252,12 @@
counts2 = Encoder2.getPulses(); // gives position of encoder
pc.printf("Resolution: %f pulses/rad \r\n",resolution);
}
-/*
+
if (BiQuadTicker_go){
BiQuadTicker_go=false;
BiQuadFilter();
}
-*/
+
}
+*/
}
\ No newline at end of file