Thom Kuenen
Biorobotics 2018: Project group 16
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 3:766e9f13d84e
- Parent:
- 2:bc6043623fb7
- Child:
- 4:a682fb1f37d2
--- a/main.cpp Thu Nov 01 10:17:18 2018 +0000
+++ b/main.cpp Thu Nov 01 10:52:03 2018 +0000
@@ -4,62 +4,68 @@
#include "QEI.h"
#include "BiQuad.h"
-MODSERIAL pc(USBTX, USBRX);
-DigitalOut DirectionPin1(D4);
-DigitalOut DirectionPin2(D7);
-PwmOut PwmPin1(D5);
-PwmOut PwmPin2(D6);
-DigitalIn Knop1(D2);
-DigitalIn Knop2(D3);
-DigitalIn Knop3(PTA4);
-DigitalIn Knop4(PTC6);
-AnalogIn pot1 (A5);
-AnalogIn pot2 (A4);
-AnalogIn emg0( A0 );
-AnalogIn emg1( A1 );
-AnalogIn emg2( A2 );
-AnalogIn emg3( A3 );
-DigitalOut led_G(LED_GREEN);
-DigitalOut led_R(LED_RED);
-DigitalOut led_B(LED_BLUE);
+// Pin defintions:
+MODSERIAL pc(USBTX, USBRX);
+DigitalOut DirectionPin1(D4);
+DigitalOut DirectionPin2(D7);
+PwmOut PwmPin1(D5);
+PwmOut PwmPin2(D6);
+DigitalIn Knop1(D2);
+DigitalIn Knop2(D3);
+DigitalIn Knop3(PTA4);
+DigitalIn Knop4(PTC6);
+AnalogIn pot1 (A5);
+AnalogIn pot2 (A4);
+AnalogIn emg0( A0 );
+AnalogIn emg1( A1 );
+AnalogIn emg2( A2 );
+AnalogIn emg3( A3 );
+DigitalOut led_G(LED_GREEN);
+DigitalOut led_R(LED_RED);
+DigitalOut led_B(LED_BLUE);
+// Encoder functions.
QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
-BiQuadChain bqc1;
-BiQuadChain bqc2;
-BiQuadChain bqc3;
-BiQuadChain bqc4;
-BiQuadChain bqc5;
-BiQuadChain bqc6;
-BiQuadChain bqc7;
-BiQuadChain bqc8;
+// Filter definitions:
+BiQuadChain bqc1;
+BiQuadChain bqc2;
+BiQuadChain bqc3;
+BiQuadChain bqc4;
+BiQuadChain bqc5;
+BiQuadChain bqc6;
+BiQuadChain bqc7;
+BiQuadChain bqc8;
-BiQuad BqNotch1_1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
-BiQuad BqNotch2_1( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
-BiQuad BqNotch1_2( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
-BiQuad BqNotch2_2( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
-BiQuad BqNotch1_3( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
-BiQuad BqNotch2_3( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
-BiQuad BqNotch1_4( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
-BiQuad BqNotch2_4( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
-BiQuad BqHP1( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
-BiQuad BqHP2( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
-BiQuad BqHP3( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
-BiQuad BqHP4( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad BqNotch1_1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
+BiQuad BqNotch2_1( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad BqNotch1_2( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
+BiQuad BqNotch2_2( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad BqNotch1_3( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
+BiQuad BqNotch2_3( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad BqNotch1_4( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
+BiQuad BqNotch2_4( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
+BiQuad BqHP1( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad BqHP2( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad BqHP3( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
+BiQuad BqHP4( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
-BiQuad BqLP1( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
-BiQuad BqLP2( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
-BiQuad BqLP3( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
-BiQuad BqLP4( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+BiQuad BqLP1( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+BiQuad BqLP2( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+BiQuad BqLP3( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+BiQuad BqLP4( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+
+BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+// Tickers for the main loop:
+Ticker StateTicker;
+Ticker printTicker;
-Ticker StateTicker;
-Ticker printTicker;
+// Setting the HIDscope:
+HIDScope scope( 4 );
-HIDScope scope( 4 );
-
+// All variables for EMG-processing:
volatile float Bicep_Right = 0.0;
volatile float Bicep_Left = 0.0;
volatile float Tricep_Right = 0.0;
@@ -77,66 +83,71 @@
volatile float Filterabs_Tri_L;
volatile float Filtered_Tri_L;
-volatile float error_1_integral = 0;
-volatile float error_2_integral = 0;
-volatile float error_1_prev; // initialization with this value only done once!
-volatile float error_2_prev;
+// Variables for the PID-Controller:
+volatile float error_1_integral = 0;
+volatile float error_2_integral = 0;
+volatile float error_1_prev; // initialization with this value only done once!
+volatile float error_2_prev;
+volatile const float Kp = 17.5;
+volatile const float Ki = 1.02;
+volatile const float Kd = 23.2;
+volatile const float Ts = 0.002; // Sample time in seconds
+volatile float error_1;
+volatile float error_2;
+volatile float U_1;
+volatile float U_2;
+// Constant values used in computations and such:
volatile const float pi = 3.1415926;
volatile const float rad_count = 0.0007479; // 2pi/8400;
volatile const float maxVelocity = 2.0f * pi; // in rad/s
volatile const float r_3 = 0.035;
-volatile float referenceVelocity1 = 0.5; // dit is de gecentreerde waarde en dus de nulstand
-volatile float referenceVelocity2 = 0.5;
-
+// Some counting variables:
volatile int i = 0; // Led blink status
volatile int ii = 0; // Calibratie timer
volatile int iii = 0; // Start up timer
-volatile float Kp = 17.5;
-volatile float Ki = 1.02;
-volatile float Kd = 23.2;
-volatile float Ts = 0.01; // Sample time in seconds
-volatile float error_1;
-volatile float error_2;
-volatile float U_1;
-volatile float U_2;
-
-volatile float q_1;
-volatile float q_2;
-volatile float r_1;
-volatile float r_2;
-volatile float w_1;
-volatile float w_2;
+// Variables for computations of joint angle and velocities:
+volatile float q_1;
+volatile float q_2;
+volatile float r_1;
+volatile float r_2;
+volatile float w_1;
+volatile float w_2;
+volatile float vx;
+volatile float vy;
+volatile float rad_m1;
+volatile float rad_m2;
+volatile int counts1;
+volatile int counts2;
-volatile float Flex_Bi_R;
-volatile float Flex_Bi_L;
-volatile float Flex_Tri_R;
-volatile float Flex_Tri_L;
-volatile float Threshold_Value;
-volatile float Threshold_Bi_R;
-volatile float Threshold_Bi_L;
-volatile float Threshold_Tri_R;
-volatile float Threshold_Tri_L;
-volatile bool Checking_Bi_R = false;
-volatile bool Checking_Bi_L = false;
-volatile bool Checking_Tri_R = false;
-volatile bool Checking_Tri_L = false;
+// Threshold values and variables:
+volatile float Flex_Bi_R;
+volatile float Flex_Bi_L;
+volatile float Flex_Tri_R;
+volatile float Flex_Tri_L;
+volatile float Threshold_Value;
+volatile float Threshold_Bi_R;
+volatile float Threshold_Bi_L;
+volatile float Threshold_Tri_R;
+volatile float Threshold_Tri_L;
+volatile bool Checking_Bi_R = false;
+volatile bool Checking_Bi_L = false;
+volatile bool Checking_Tri_R = false;
+volatile bool Checking_Tri_L = false;
+// States for the switch-statemachine:
enum states{Starting, Calibration, Homing_M1, Homing_M2, Post_Homing, Function, Safe};
-
+
+// The very first state:
volatile states Active_State = Starting;
-volatile float vx;
-volatile float vy;
-volatile int counts1;
-volatile int counts2;
-volatile float rad_m1;
-volatile float rad_m2;
-
+
void Encoding()
{
+ // Encoding is necessary for the computations of the joint angles and
+ // velocities of the arm given certain linear velocities.
counts1 = Encoder1.getPulses();
counts2 = Encoder2.getPulses();
@@ -147,6 +158,9 @@
void Filter()
{
+ // Our Filter function will take the raw (incomming) EMG-signal and process
+ // it in such a way that we can use it to opperate our system.
+
FilterHP_Bi_R = bqc1.step( emg0.read() );
Filterabs_Bi_R = fabs(FilterHP_Bi_R);
Filtered_Bi_R = bqc2.step( Filterabs_Bi_R );
@@ -166,6 +180,9 @@
void BlinkLed()
{
+ // This is a function used purely for feedback that will give insight in
+ // what the system is doing without having to fully opperate it.
+
if(i==1)
{
led_G=led_B=1;
@@ -232,6 +249,9 @@
void EMG_Read()
{
+ // This is only to define our first (raw) EMG-signals and give each a
+ // suitable name.
+
Bicep_Right = emg0.read();
Bicep_Left = emg1.read();
Tricep_Right = emg2.read();
@@ -240,6 +260,8 @@
void sample()
{
+ // HIDscope allows us to check our filtered EMG-signal and gives insight
+ // into thresholdvalues and how well the calibration was done.
scope.set(0, Filtered_Bi_R*10.0f );
scope.set(1, Filtered_Bi_L*10.0f );
@@ -251,6 +273,9 @@
void Inverse()
{
+ // Here we calculate the movement of each joint (Motor 1 and Motor 2) given
+ // a certain linear velocity-input.
+
q_1= rad_m1+(pi/6.0f); // uit Encoder
q_2= rad_m2+(pi/6.0f); // uit Encoder
r_1= -0.2f;
@@ -277,6 +302,9 @@
void Calibrating()
{
+ // Calibration is done to ensure that threshold values not predetermined and
+ // can vary each user. This makes the system more universal.
+
static float n = 0.0;
static float m = 0.0;
static float l = 0.0;
@@ -369,7 +397,8 @@
pc.printf("You can relax your left tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\nThe calibration has been completed, the system is now operatable. \r\n",Flex_Tri_L);
i = 2;
}
-
+
+// Setting the Threshold:
Threshold_Value = 0.8f;
Threshold_Bi_R = Threshold_Value * Flex_Bi_R;
@@ -383,7 +412,7 @@
}
else if (ii == 20000)
{
- pc.printf("\r\nAutomatic switch to Homing State\r\n");
+ pc.printf("\r\nAutomatic switch to Homing State for Motor 1\r\n");
Active_State = Homing_M1;
i = 0;
}
@@ -393,6 +422,11 @@
void Start_Up()
{
+ // This function is active only when the system is started up or when the
+ // system has entered Sleep_Mode (iii > 40 000).
+ // It is not possible to get out of this function without some opperation on
+ // the Mbed.
+
i++;
iii++;
if (iii == 1)
@@ -418,18 +452,24 @@
void OFF_m1()
{
+ // This function ensures that Motor 1 is turned off.
+
PwmPin1 = 0;
}
void OFF_m2()
{
+ // This function ensures that Motor 2 is turned off.
+
PwmPin2 = 0;
}
void Going_Home_Motor1()
{
+ // This is the Homing function for Motor 1.
+
if (counts1 == 0)
{
- pc.printf("Switching to Homing state for motor 2\r\n");
+ pc.printf("Switching to Homing State for Motor 2\r\n");
Active_State = Homing_M2; // Statement here instead of statemachine because of checking speed
}
else if (counts1 > 0)
@@ -446,7 +486,8 @@
void Going_Home_Motor2()
{
- pc.printf("Entered homing state 2");
+ // This is the Homing function for Motor 2.
+
if (counts2 == 0)
{
// als het goed is hoeft hier niets te staan // Statement in statemachine because of the double check
@@ -465,7 +506,7 @@
void Checking_EMG()
{
- // This function will make the the setting of signal to movement easier
+ // This function will make the the setting of signal to movement easier.
if (Filtered_Bi_R >= Threshold_Bi_R)
{
@@ -487,7 +528,7 @@
void Setting_Movement()
{
- // Here we will set the emg values to the movement of the arm
+ // Here we will set the emg values to the movement of the arm.
if (Checking_Bi_R && Checking_Bi_L) // sloping y = x, y > 0
{
@@ -533,6 +574,9 @@
void PID_controller()
{
+ // The PID-controller reduces the error between the reference signal
+ // and the actual value.
+
error_1 = (w_1*0.002f) - rad_m1;
error_2 = (w_2*0.002f) - rad_m2;
@@ -565,7 +609,9 @@
}
void motor1()
-{
+{
+ // This is where Motor 1 is opperated.
+
float u = U_1;
DirectionPin1 = u < 0.0f;
PwmPin1 = fabs(u);
@@ -573,6 +619,8 @@
void motor2()
{
+ // This is where Motor 2 is opperated.
+
float u = U_2;
DirectionPin2 = u > 0.0f;
PwmPin2 = fabs(u);
@@ -580,19 +628,22 @@
void Printing()
{
+ // This function is merely for printing feedback from the system to our
+ // screen when we wish to see or check something.
+
float v1 = PwmPin1 * maxVelocity;
float v2 = PwmPin2 * maxVelocity;
- if (Active_State == Function || Active_State == Homing_M1)
+ if (Active_State == Function )//|| Active_State == Homing_M1)
{
pc.printf("q1 = %f [rad] \r\nq2 = %f [rad] \r\ncount1= %i\r\ncount2= %i\r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n\r\n\r\n\r\n", rad_m1, rad_m2,counts1, counts2, v1, v2);
}
-
- pc.printf("%i \r\n%i \r\n\r\n",counts1,counts2);
}
void EMG_test() // even nalopen of dit ook kan met i++!
{
+ // Here we can check the opperation of the system for checking purposes.
+
led_G=led_R=led_B=1;
if (Filtered_Bi_R >= Threshold_Bi_R)
@@ -621,6 +672,26 @@
void StateMachine()
{
+ // In the Statemachine every function is integrated into the system
+ // depending on the state that the system is in. This is makes the
+ // integration and the opperation of the system a lot simpeler.
+
+ // We have 5 main states:
+ // - Starting
+ // - Start_Up
+ // - Sleep_Mode
+ // - Calibration
+ // - Homing
+ // - Homing_M1
+ // - Homing_M2
+ // - Post_Homing
+ // - Function
+ // - Safe
+
+ // As seen above some states have substates that make the system run better
+ // or give the state some unique features. These are explained within the
+ // functions that are called in that state.
+
switch (Active_State)
{
case Starting:
@@ -783,6 +854,11 @@
int main()
{
+ // Our main loop is as empty as possible and contains only statements that
+ // cannot be put elsewhere due to functioning reasons.
+ // Also the "while-loop" is completely empty, which improves the running
+ // speed of the system.
+
pc.baud(115200);
PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz