Henk van Gansen
Vanmorgen aan gewerkt
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
Project_BioRobotics
by 
Jordi Luong
Diff: main.cpp
- Revision:
- 12:12b72bd60fd1
- Parent:
- 11:5c06c97c3673
- Child:
- 13:b455e2e8bed2
--- a/main.cpp Mon Oct 30 12:11:42 2017 +0000
+++ b/main.cpp Wed Nov 01 06:46:29 2017 +0000
@@ -10,6 +10,7 @@
// SERIAL COMMUNICATION WITH PC ////////////////////////////////////////////////
MODSERIAL pc(USBTX, USBRX);
+HIDScope scope(4);
// STATES //////////////////////////////////////////////////////////////////////
enum states{MOTORS_OFF, MOVING, HITTING};
@@ -29,54 +30,57 @@
InterruptIn button2(D3); // CONNECT BUT2 TO D3
InterruptIn button3(SW2);
InterruptIn button4(SW3);
-AnalogIn emg(A0);
+//AnalogIn emg(A0);
+AnalogIn potmeter1(A0);
+AnalogIn potmeter2(A1);
// MOTOR CONTROL ///////////////////////////////////////////////////////////////
Ticker controllerTicker;
-const double controller_Ts = 0.01; // Time step for controllerTicker [s]; Should be between 5kHz and 10kHz
+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.3; // X and Y velocity of the end effector when desired
+double velocity = 0.07; // X and Y velocity of the end effector when desired
// MOTOR 1
-double position1; // Position of motor 1 [rad]
-double reference1 = 0; // Desired rotation of motor 1 [rad]
-double motor1Max = 2*pi*45/360; // Maximum rotation of motor 1 [rad]
-double motor1Min = 0; // Minimum rotation of motor 1 [rad]
+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 = 5; // Position gain []
-const double motor1_KI = 5; // Integral gain []
-const double motor1_KD = 0.0; // Derivative gain []
+const double motor1_KP = 7; // Position gain []
+const double motor1_KI = 9; // Integral gain []
+const double motor1_KD = 1; // Derivative gain []
double motor1_err_int = 0, motor1_prev_err = 0;
// Derivative filter coefficients
-const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0; // Derivative filter coefficients []
-const double motor1_f_b0 = 1.0, motor1_f_b1 = 3.0, motor1_f_b2 = 4.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
-double position2; // Position of motor 2 [rad]
+volatile double position2; // Position of motor 2 [rad]
volatile double reference2 = 0; // Desired rotation of motor 2 [rad]
-double motor2Max = 2*pi*45/360; // Maximum rotation of motor 2 [rad]
-double motor2Min = 2*pi*-45/360; // Minimum 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]
// Controller gains
-const double motor2_KP = 3; // Position gain []
-const double motor2_KI = 3; // Integral gain []
-const double motor2_KD = 0.0; // Derivative gain []
+const double motor2_KP = potmeter1*10; // Position gain []
+const double motor2_KI = potmeter2*20; // Integral gain []
+const double motor2_KD = 1; // Derivative gain []
double motor2_err_int = 0, motor2_prev_err = 0;
// Derivative filter coefficients
-const double motor2_f_a1 = 1.0, motor2_f_a2 = 2.0; // Derivative filter coefficients []
-const double motor2_f_b0 = 1.0, motor2_f_b1 = 3.0, motor2_f_b2 = 4.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(0, 0, 0, 0, 0); // EMG filter coefficients []
-BiQuad bq2(0, 0, 0, 0, 0); // EMG filter coefficients []
-Ticker emgSampleTicker;
-double emg_Ts = 0.1; // Time step for EMG sampling [s]
+//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 ///////////////////////////////////////////////////////////////////
@@ -90,6 +94,7 @@
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,
@@ -98,13 +103,13 @@
{
// Derivative
double e_der = (e - e_prev)/Ts; // Calculate the derivative of error
- //pc.printf("derivative error %f \r\n", e_der);
- //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); // Filter the derivative of error
+ e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); // Filter the derivative of error
+ //e_der = bq1.step(e_der);
e_prev = e;
// Integral
e_int = e_int + Ts*e; // Calculate the integral of error
// PID
- //pc.printf("Na derivative error %f \r\n", e_der);
+ //pc.printf("%f --> %f \r\n", e_der, e_derf);
return Kp*e + Ki*e_int + Kd*e_der;
}
@@ -140,28 +145,32 @@
void Motor1Controller()
{
position1 = radPerPulse * Encoder1.getPulses();
- pc.printf("error %f \r\n", reference1 - position1);
+ position2 = radPerPulse * Encoder2.getPulses();
+ //pc.printf("error %f \r\n", reference1 - position1);
double motor1Value = PID_Controller(reference1 - position1, motor1_KP,
motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err,
motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
motor1_f_b1, motor1_f_b2);
//pc.printf("motor value %f \r\n",motor1Value);
RotateMotor1(motor1Value);
-}
-
-// MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
-void Motor2Controller()
-{
- position2 = radPerPulse * Encoder2.getPulses();
- //pc.printf("%f", position2);
double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
motor2_KI, motor2_KD, controller_Ts, motor2_err_int, motor2_prev_err,
motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
motor2_f_b1, motor2_f_b2);
- pc.printf("motor value %f \r\n",motor2Value);
RotateMotor2(motor2Value);
}
+// MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
+/*void Motor2Controller()
+{
+ position2 = radPerPulse * Encoder2.getPulses();
+ double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
+ motor2_KI, motor2_KD, controller2_Ts, motor2_err_int, motor2_prev_err,
+ motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
+ motor2_f_b1, motor2_f_b2);
+ RotateMotor2(motor2Value);
+}
+*/
// TURN OFF MOTORS /////////////////////////////////////////////////////////////
void TurnMotorsOff()
{
@@ -175,12 +184,12 @@
position1 = radPerPulse * Encoder1.getPulses();
position2 = radPerPulse * Encoder2.getPulses();
- if(!button1) xVelocity = velocity;//&& position1 > motor1Min && position2 > motor2Min
- else if(!button2) xVelocity = -velocity;//&& position1 < motor1Max && position2 < motor2Max
+ if(!button1 && reference1 > motor1Min && reference2 < motor2Max) xVelocity = velocity;
+ else if(!button2 && reference1 < motor1Max && reference2 > motor2Min) xVelocity = -velocity;
else xVelocity = 0;
- if(!button3) yVelocity = velocity;//&& position1 < motor1Max && position2 < motor2Max
- else if(!button4) yVelocity = -velocity;//&& position1 < motor1Max && position2 > motor2Min
+ if(!button3 && reference2 < motor2Max && reference1 > motor2Min) yVelocity = velocity;
+ else if(!button4 && reference2 > motor2Min && reference1 > motor2Min) yVelocity = -velocity;
else yVelocity = 0;
//pc.printf("x %f, y %f \r\n", xVelocity, yVelocity);
@@ -246,19 +255,24 @@
msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1];
msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3];
- if(position1 + msh[0]*emg_Ts > motor1Max) reference1 = motor1Max;
- else if(position1 + msh[0]*emg_Ts < motor1Min) reference1 = motor1Min;
- else reference1 = position1 + msh[0]*emg_Ts;
+ if(reference1 + msh[0]*tickerTs > motor1Max) reference1 = motor1Max;
+ else if(reference1 + msh[0]*tickerTs < motor1Min) reference1 = motor1Min;
+ else reference1 = reference1 + msh[0]*tickerTs;
- if(position2 + msh[1]*emg_Ts > motor2Max) reference2 = motor2Max;
- else if(position2 + msh[1]*emg_Ts < motor2Min) reference2 = motor2Min;
- else reference2 = position2 + msh[1]*emg_Ts;
+ if(reference2 + msh[1]*tickerTs > motor2Max) reference2 = motor2Max;
+ else if(reference2 + msh[1]*tickerTs < motor2Min) reference2 = motor2Min;
+ else reference2 = reference2 + msh[1]*tickerTs;
- Motor1Controller(), Motor2Controller();
+ //Motor1Controller(), Motor2Controller();
+ scope.set(0,reference1);
+ scope.set(1,position1);
+ scope.set(2,reference2);
+ scope.set(3,position2);
+ scope.send();
pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360);
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);
+ //pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts);
}
// EMG /////////////////////////////////////////////////////////////////////////
@@ -287,8 +301,8 @@
// State initialization
if(stateChanged)
{
- pc.printf("Entering MOTORS_OFF \r\n"
- "Press button 1 to enter MOVING \r\n");
+ //pc.printf("Entering MOTORS_OFF \r\n"
+ //"Press button 1 to enter MOVING \r\n");
TurnMotorsOff(); // Turn motors off
stateChanged = false;
}
@@ -308,8 +322,8 @@
// State initialization
if(stateChanged)
{
- pc.printf("Entering MOVING \r\n"
- "Press button 2 to enter HITTING \r\n");
+ //pc.printf("Entering MOVING \r\n"
+ //"Press button 2 to enter HITTING \r\n");
stateChanged = false;
}
@@ -329,7 +343,7 @@
// State initialization
if(stateChanged)
{
- pc.printf("Entering HITTING \r\n");
+ //pc.printf("Entering HITTING \r\n");
stateChanged = false;
//HitBall(); // Hit the ball
currentState = MOVING;
@@ -355,10 +369,14 @@
pc.printf("START \r\n");
- bqc.add(&bq1).add(&bq2); // Make BiQuad Chain
+ //bqc.add(&bq1).add(&bq2); // Make BiQuad Chain
- //controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID)
- emgSampleTicker.attach(&EmgSample, emg_Ts); // Ticker to sample EMG
+ sampleTicker.attach(&EmgSample, tickerTs); // Ticker to sample EMG
+ controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID)
+
+ motor1MagnitudePin.period_ms(1);
+ motor2MagnitudePin.period_ms(1);
+ TurnMotorsOff();
while(true)
{