Jordi Luong
Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 13:ec227b229f3d
- Parent:
- 12:12b72bd60fd1
- Child:
- 14:95acac6a07c7
- Child:
- 22:68e3dc374488
--- a/main.cpp Wed Nov 01 06:46:29 2017 +0000
+++ b/main.cpp Wed Nov 01 11:25:22 2017 +0000
@@ -5,22 +5,22 @@
#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
-
+
const double pi = 3.1415926535897; // Definition of pi
-
+
// SERIAL COMMUNICATION WITH PC ////////////////////////////////////////////////
MODSERIAL pc(USBTX, USBRX);
HIDScope scope(4);
-
+
// STATES //////////////////////////////////////////////////////////////////////
enum states{MOTORS_OFF, MOVING, HITTING};
states currentState = MOTORS_OFF; // Start with motors off
bool stateChanged = true; // Make sure the initialization of first state is executed
-
+
// ENCODER /////////////////////////////////////////////////////////////////////
QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D10, ENC1B TO D11
QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D12, ENC2B TO D13
-
+
// PINS ////////////////////////////////////////////////////////////////////////
DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW
PwmOut motor1MagnitudePin(D5);
@@ -33,56 +33,58 @@
//AnalogIn emg(A0);
AnalogIn potmeter1(A0);
AnalogIn potmeter2(A1);
-
+
// MOTOR CONTROL ///////////////////////////////////////////////////////////////
Ticker controllerTicker;
const double controller_Ts = 1/200.1; // Time step for controllerTicker [s]; Should be between 5kHz and 10kHz
const double motorRatio = 131.25; // Ratio of the gearbox in the motors []
const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse]
double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start
-double velocity = 0.07; // X and Y velocity of the end effector when desired
-
+double velocity = 0.05; // X and Y velocity of the end effector when desired
+
// MOTOR 1
volatile double position1; // Position of motor 1 [rad]
volatile double reference1 = 0; // Desired rotation of motor 1 [rad]
double motor1Max = 0; // Maximum rotation of motor 1 [rad]
double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad]
// Controller gains
-const double motor1_KP = 7; // Position gain []
-const double motor1_KI = 9; // Integral gain []
-const double motor1_KD = 1; // Derivative gain []
+const double motor1_KP = 13; // Position gain []
+const double motor1_KI = 7; // Integral gain []
+const double motor1_KD = 0.3; // Derivative gain []
double motor1_err_int = 0, motor1_prev_err = 0;
// Derivative filter coefficients
const double motor1_f_a1 = 0.25, motor1_f_a2 = 0.8; // Derivative filter coefficients []
const double motor1_f_b0 = 1, motor1_f_b1 = 2, motor1_f_b2 = 0.8; // Derivative filter coefficients []
// Filter variables
double motor1_f_v1 = 0, motor1_f_v2 = 0;
-
+
// MOTOR 2
volatile double position2; // Position of motor 2 [rad]
volatile double reference2 = 0; // Desired rotation of motor 2 [rad]
-double motor2Max = 2*pi*30/360; // Maximum rotation of motor 2 [rad]
-double motor2Min = 2*pi*-30/360; // Minimum rotation of motor 2 [rad]
+double motor2Max = 2*pi*25/360; // Maximum rotation of motor 2 [rad]
+double motor2Min = 2*pi*-28/360; // Minimum rotation of motor 2 [rad]
// Controller gains
-const double motor2_KP = potmeter1*10; // Position gain []
-const double motor2_KI = potmeter2*20; // Integral gain []
-const double motor2_KD = 1; // Derivative gain []
+//const double motor2_KP = potmeter1*10; // Position gain []
+//const double motor2_KI = potmeter2*20; // Integral gain []
+const double motor2_KP = 13; // Position gain []
+const double motor2_KI = 5; // Integral gain []
+const double motor2_KD = 0.1; // Derivative gain []
double motor2_err_int = 0, motor2_prev_err = 0;
// Derivative filter coefficients
const double motor2_f_a1 = 0.25, motor2_f_a2 = 0.8; // Derivative filter coefficients []
const double motor2_f_b0 = 1, motor2_f_b1 = 2, motor2_f_b2 = 0.8; // Derivative filter coefficients []
// Filter variables
double motor2_f_v1 = 0, motor2_f_v2 = 0;
-
+
// EMG FILTER //////////////////////////////////////////////////////////////////
//BiQuadChain bqc;
//BiQuad bq1(3.6, 5.0, 2.0, 0.5, 30.0); // EMG filter coefficients []
//BiQuad bq2(0, 0, 0, 0, 0); // EMG filter coefficients []
-
+
Ticker sampleTicker;
const double tickerTs = 0.1; // Time step for sampling [s]
-
-
+
+
// FUNCTIONS ///////////////////////////////////////////////////////////////////
// BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
double biquad(double u, double &v1, double &v2, const double a1,
@@ -93,8 +95,8 @@
v2 = v1; v1 = v;
return y;
}
-
-
+
+
// PID-CONTROLLER WITH FILTER //////////////////////////////////////////////////
double PID_Controller(double e, const double Kp, const double Ki,
const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
@@ -112,7 +114,7 @@
//pc.printf("%f --> %f \r\n", e_der, e_derf);
return Kp*e + Ki*e_int + Kd*e_der;
}
-
+
// MOTOR 1 /////////////////////////////////////////////////////////////////////
void RotateMotor1(double motor1Value)
{
@@ -126,7 +128,7 @@
}
else motor1MagnitudePin = 0;
}
-
+
// MOTOR 2 /////////////////////////////////////////////////////////////////////
void RotateMotor2(double motor2Value)
{
@@ -140,7 +142,7 @@
}
else motor2MagnitudePin = 0;
}
-
+
// MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
void Motor1Controller()
{
@@ -159,7 +161,7 @@
motor2_f_b1, motor2_f_b2);
RotateMotor2(motor2Value);
}
-
+
// MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
/*void Motor2Controller()
{
@@ -177,7 +179,7 @@
motor1MagnitudePin = 0;
motor2MagnitudePin = 0;
}
-
+
// DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
void jointVelocities()
{
@@ -187,7 +189,7 @@
if(!button1 && reference1 > motor1Min && reference2 < motor2Max) xVelocity = velocity;
else if(!button2 && reference1 < motor1Max && reference2 > motor2Min) xVelocity = -velocity;
else xVelocity = 0;
-
+
if(!button3 && reference2 < motor2Max && reference1 > motor2Min) yVelocity = velocity;
else if(!button4 && reference2 > motor2Min && reference1 > motor2Min) yVelocity = -velocity;
else yVelocity = 0;
@@ -274,7 +276,7 @@
pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360);
//pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts);
}
-
+
// EMG /////////////////////////////////////////////////////////////////////////
void EmgSample()
{
@@ -290,7 +292,7 @@
jointVelocities();
}
}
-
+
// STATES //////////////////////////////////////////////////////////////////////
void ProcessStateMachine()
{
@@ -360,7 +362,7 @@
}
}
}
-
+
// MAIN FUNCTION ///////////////////////////////////////////////////////////////
int main()
{