Dependencies: FastPWM MODSERIAL QEI biquadFilter mbed
Fork of
state_program
by 
Jesse Lohman
Diff: main.cpp
- Revision:
- 3:be922ea2415f
- Parent:
- 2:5cace74299e7
- Child:
- 4:8f25ecb74221
--- a/main.cpp Tue Oct 30 09:15:02 2018 +0000
+++ b/main.cpp Wed Oct 31 12:15:34 2018 +0000
@@ -3,6 +3,8 @@
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
+#include <iostream>
+#include <string>
enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
States currentState = WaitState;
@@ -18,9 +20,10 @@
DigitalOut led2(LED2); // Green led
DigitalOut led3(LED3); // Blue led
QEI encoder1(D14, D15, NC, 8400, QEI::X4_ENCODING);
-//QEI encoder2(A2, A3), NC, 4200);
+QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING);
//QEI encoder3(A4, A5), NC, 4200);
AnalogIn pot(A0); // Speed knob
+AnalogIn pot2(A1);
bool stateChanged = true;
@@ -30,14 +33,52 @@
const double sampleTime = 0.001;
const float maxVelocity=8.4; // in rad/s
const double PI = 3.141592653589793238463;
-
-double u1, u2, u3, u4; // u1 is motor output of the long link, u2 is motor of the short link, u3 is motor of gripper, u4 is motor of screwer
+const double L1 = 0.328;
+const double L2 = 0.218;
+ double T1[3][3] {
+ {0, -1, 0},
+ {1, 0, 0,},
+ {0, 0, 0,}
+};
+ double T20[3][3] {
+ {0, -1, 0},
+ {1, 0, -L1,},
+ {0, 0, 0,}
+};
+ double H200[3][3] {
+ {1, 0, L1+L2},
+ {0, 1, 0,},
+ {0, 0, 1,}
+};
+ double Pe2 [3][1] {
+ {0},
+ {0},
+ {1}
+};
+
+double u1;
+double u2; // u1 is motor output of the long link, u2 is motor of the short link, u3 is motor of gripper, u4 is motor of screwer
+double u3;
+double u4;
FastPWM pwmpin1(D5); //motor pwm
DigitalOut directionpin1(D4); // motor direction
-double robotEndPoint;
+FastPWM pwmpin2 (D6);
+DigitalOut directionpin2 (D7);
+double setPointX;
+double setPointY;
+double qRef1;
+double qRef2;
+double qMeas1;
+double qMeas2;
-double v;
-double Dpulses;
+double v; // Global variable for printf
+double Dpulses; // Global variable for printf
+
+double C[5][5];
+
+double Kp = 0.1;
+double Ki = 0;
+double Kd = 0;
void switchToFailureState ()
{
@@ -55,26 +96,26 @@
static double lastPulses = 0;
double currentPulses;
static double velocity = 0;
-
+
static int i = 0;
if (i == 10) { // Encoder is not accurate enough, so with 1000 Hz the velocity can only be 0, 1000, 2000 or 3000 pulses/s
- switch (motor) { // Check which motor to measure
- case 1:
- currentPulses = encoder1.getPulses();
- break;
- case 2:
- //currentPulses = encoder2.getPulses();
- break;
- case 3:
- //currentPulses = encoder3.getPulses();
- break;
- }
+ switch (motor) { // Check which motor to measure
+ case 1:
+ currentPulses = encoder1.getPulses();
+ break;
+ case 2:
+ //currentPulses = encoder2.getPulses();
+ break;
+ case 3:
+ //currentPulses = encoder3.getPulses();
+ break;
+ }
- double deltaPulses = currentPulses - lastPulses;
- Dpulses = deltaPulses;
- velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
- lastPulses = currentPulses;
- i = 0;
+ double deltaPulses = currentPulses - lastPulses;
+ Dpulses = deltaPulses;
+ velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s
+ lastPulses = currentPulses;
+ i = 0;
} else {
i += 1;
}
@@ -82,22 +123,212 @@
return velocity;
}
-void measureAll ()
+void measurePosition() // Measure actual angle position with the encoder
{
+ double pulses1 = encoder1.getPulses();
+ double pulses2 = encoder2.getPulses();
+ qMeas1 = pulses1 * 2 * PI / 8400 + 840; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset
+ qMeas2 = pulses2 * 2 * PI / 8400 + 70;
}
void getMotorControlSignal () // Milestone 1 code, not relevant anymore
{
- double potSignal = pot.read(); // read pot and scale to motor control signal
- //pc.printf("motor control signal = %f\n", potSignal);
+ double potSignal = pot.read() * 2 - 1; // read pot and scale to motor control signal
+ //pc.printf("motor control signal = %f\n", posampleTimeignal);
u1 = potSignal;
+ u2 = potSignal;
+}
+
+template<std::size_t N, std::size_t M, std::size_t P>
+void mult(double A[N][M], double B[M][P])
+{
+
+ for( int n =0; n < 5; n++) {
+ for(int p =0; p < 5; p++) {
+ C[n][p] =0;
+ }
+ }
+ for (int n = 0; n < N; n++) {
+ for (int p = 0; p < P; p++) {
+ double num = 0;
+ for (int m = 0; m < M; m++) {
+ num += A[n][m] * B[m][p];
+
+ }
+
+ C[n][p] = num;
+
+ }
+ }
+
+}
+
+void inverseKinematics ()
+{
+ if (currentState == MovingState) { // Only in the HomingState should the qRef1, qRef2 consistently change
+ double potx = 0.218;//pot1.read()*0.546;
+ double poty = 0.328;//pot2.read()*0.4;
+
+ double Pe_set[3][1] { // defining setpoint location of end effector
+ {potx}, //setting xcoord to pot 1
+ {poty}, // setting ycoord to pot 2
+ {1}
+ };
+
+//Calculating new H matrix
+ double T1e[3][3] {
+ {cos(qRef1), -sin(qRef1), 0},
+ {sin(qRef1), cos(qRef1), 0},
+ {0, 0, 1}
+ };
+ double T20e[3][3] {
+ {cos(qRef2), -sin(qRef2), L1-L1*cos(qRef2)},
+ {sin(qRef2), cos(qRef2), -L1*sin(qRef2)},
+ {0, 0, 1}
+ };
+
+
+ mult<3,3,3>(T1e,T20e); // matrix multiplication
+ double H201[3][3] {
+ {C[0][0],C[0][1],C[0][2]},
+ {C[1][0],C[1][1],C[1][2]},
+ {C[2][0],C[2][1],C[2][2]}
+ };
+
+ mult<3,3,3>(H201,H200); // matrix multiplication
+ double H20 [3][3] {
+ {C[0][0],C[0][1],C[0][2]},
+ {C[1][0],C[1][1],C[1][2]},
+ {C[2][0],C[2][1],C[2][2]}
+ };
+
+ mult<3,3,1>(H20,Pe2); // matrix multiplication
+ double Pe0[3][1] {
+ {C[0][0]},
+ {C[1][0]},
+ {C[2][0]}
+ };
+
+ double pe0x = Pe0[0][0]; // seperating coordinates of end effector location
+ double pe0y = Pe0[1][0];
+
+ // Determing the jacobian
+
+ double T_1[3][1] {
+ {1},
+ {T1[0][2]},
+ {T1[1][2]}
+ };
+
+ double T_2[3][1] {
+ {1},
+ {L1*sin(qRef1)},
+ {-L1*cos(qRef1)}
+ };
+
+ double J[3][2] {
+ {T_1[0][0], T_2[0][0]},
+ {T_1[1][0], T_2[1][0]},
+ {T_1[2][0], T_2[2][0]}
+ };
+
+//Determing 'Pulling" force to setpoint
+
+ double k= 1; //virtual stifness of the force
+ double Fs[3][1] { //force vector from end effector to setpoint
+ {k*Pe_set[0][0] - k*Pe0[0][0]},
+ {k*Pe_set[1][0] - k*Pe0[1][0]},
+ {k*Pe_set[2][0] - k*Pe0[2][0]}
+ };
+ double Fx = k*potx - k*pe0x;
+ double Fy = k*poty - k*pe0y;
+ double W0t[3][1] {
+ {pe0x*Fy - pe0y*Fx},
+ {Fx},
+ {Fy}
+ };
+
+ double Jt[2][3] { // transposing jacobian matrix
+ {J[0][0], J[1][0], J[2][0]},
+ {T_2[0][0], T_2[1][0], T_2[2][0]}
+ };
+
+ mult<2,3,1>(Jt,W0t);
+ double tau_st1 = C[0][0];
+ double tau_st2 = C[1][0];
+
+//Calculating joint behaviour
+
+ double b =1;
+ //joint friction coefficent
+ //double sampleTime = 1/1000; //Time step to reach the new angle
+ double w_s1 = tau_st1/b; // angular velocity
+ double w_s2 = tau_st2/b; // angular velocity
+ //checking angle boundaries
+ qRef1 = qRef1 +w_s1*sampleTime; // calculating new angle of qRef1 in time step sampleTime
+ if (qRef1 > 2*PI/3) {
+ qRef1 = 2*PI/3;
+ } else if (qRef1 < PI/6) {
+ qRef1= PI/6;
+ }
+
+ qRef2 = qRef2 + w_s2*sampleTime; // calculating new angle of qRef2 in time step sampleTime
+ if (qRef2 > -PI/4) {
+ qRef2 = -PI/4;
+ } else if (qRef2 < -PI/2) {
+ qRef2= -PI/2;
+ }
+ }
+}
+
+void PID_controller() // Put the error trough PID control to make output 'u'
+{
+ if (currentState >= HomingState && currentState < FailureState) { // Should only work when we move the robot to a defined position
+ double error1 = qRef1 - qMeas1;
+ double error2 = qRef2 - qMeas2;
+
+ static double errorIntegral1 = 0;
+ static double errorIntegral2 = 0;
+ static double errorPrev1 = error1;
+ static double errorPrev2 = error2;
+ static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+ //Ki = pot2.read() * 0.5; //Only Kd is controlled by a pot, Kp and Ki are constant
+
+ // Proportional part:
+ Kp = pot2.read() * 1;
+ double u_k1 = Kp * error1;
+ double u_k2 = Kp * error2;
+
+ //Integral part:
+ errorIntegral1 = errorIntegral1 + error1 * sampleTime;
+ double u_i1 = Ki * errorIntegral1;
+ errorIntegral2 = errorIntegral2 + error2 * sampleTime;
+ double u_i2 = Ki * errorIntegral2;
+
+ // Derivative part
+ double errorDerivative1 = (error1 - errorPrev1)/sampleTime;
+ double filteredErrorDerivative1 = LowPassFilter.step(errorDerivative1);
+ double u_d1 = Kd * filteredErrorDerivative1;
+ errorPrev1 = error1;
+ double errorDerivative2 = (error2 - errorPrev2)/sampleTime;
+ double filteredErrorDerivative2 = LowPassFilter.step(errorDerivative2);
+ double u_d2 = Kd * filteredErrorDerivative2;
+ errorPrev2 = error2;
+
+ // Sum all parsampleTime
+ u1 = u_k1 + u_i1 + u_d1;
+ u2 = u_k2 + u_i2 + u_d2;
+ }
+
}
void controlMotor () // Control direction and speed
{
directionpin1 = u1 > 0.0f; // Either true or false
pwmpin1 = fabs(u1);
+ directionpin2 = u2 > 0.0f; // Either true or false
+ pwmpin2 = fabs(u2);
}
void stateMachine ()
@@ -129,8 +360,8 @@
// Entry action: all the things you do once in this state
led2 = 0;
// Set motorpwm to 'low' value
- u1 = 0.6; //TODO: Check if direction is right
- u2 = 0.6;
+ //u1 = 0.6; //TODO: Check if direction is right
+ //u2 = 0.6;
stateTimer.reset();
stateTimer.start();
stateChanged = false;
@@ -139,7 +370,7 @@
// Add stuff you do every loop
getMotorControlSignal();
- if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && screwingSwitch.read() == false) { //TODO: add && fabs(measureVelocity(2)) < 0.1f
+ if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && screwingSwitch.read() == false) { //TODO: add && fabs(measureVelocity(2)) < 0.1f
//TODO: Add reset of encoder2
led2 = 1;
encoder1.reset(); // Reset encoder for the 0 position
@@ -171,8 +402,10 @@
if (stateChanged == true) {
// Entry action: all the things you do once in this state;
led1 = 0;
- led2 = 0; // Emits yellow together
- //TODO: Set intended position
+ led2 = 0; // EmisampleTime yellow together
+ //TODO: Set qRef1 and qRef2
+ qRef1 = 90 * PI / 180;
+ qRef2 = -90 * PI / 180;
stateChanged = false;
}
@@ -190,7 +423,7 @@
// Entry action: all the things you do once in this state;
led1 = 0;
led2 = 0;
- led3 = 0; // Emits white together
+ led3 = 0; // EmisampleTime white together
stateChanged = false;
}
@@ -207,7 +440,7 @@
if (stateChanged == true) {
// Entry action: all the things you do once in this state;
led2 = 0;
- led3 = 0; // Emits light blue together
+ led3 = 0; // EmisampleTime light blue together
stateChanged = false;
}
@@ -234,7 +467,7 @@
if (stateChanged == true) {
// Entry action: all the things you do once in this state;
led1 = 0;
- led3 = 0; // Emits pink together
+ led3 = 0; // EmisampleTime pink together
stateChanged = false;
}
@@ -272,11 +505,18 @@
}
}
+void measureAll ()
+{
+ measurePosition();
+ inverseKinematics();
+}
+
void mainLoop ()
{
// Add measure, motor controller and output function
measureAll();
stateMachine();
+ PID_controller();
controlMotor();
}