Jordi Luong
Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 20:ab391a133a01
- Parent:
- 19:6f720e4fcb47
- Child:
- 21:2e732eb85daf
diff -r 6f720e4fcb47 -r ab391a133a01 main.cpp
--- a/main.cpp Fri Nov 03 09:47:09 2017 +0000
+++ b/main.cpp Fri Nov 03 10:04:52 2017 +0000
@@ -10,7 +10,6 @@
// SERIAL COMMUNICATION WITH PC ////////////////////////////////////////////////
MODSERIAL pc(USBTX, USBRX);
-//HIDScope scope(4);
// STATES //////////////////////////////////////////////////////////////////////
enum states{MOTORS_OFF, CALIBRATING, MOVING};
@@ -83,7 +82,7 @@
// EMG /////////////////////////////////////////////////////////////////////////
Ticker emgLeft; // Ticker for EMG of left arm
Ticker emgRight; // Ticker for EMG of right arm
-const double emgTs = 0.4993; // Time step for EMG sampling
+const double emgTs = 0.4993; // Time step for EMG sampling
// Filters
BiQuadChain bqc;
BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004); // High pass filter
@@ -108,7 +107,7 @@
// PROCESS EMG SIGNALS /////////////////////////////////////////////////////////
Ticker processTicker; // Ticker for processing of EMG
-const double processTs = 0.101; // Time step for processing of EMG
+const double processTs = 0.101; // Time step for processing of EMG
volatile bool xdir = true, ydir = false; // Direction the EMG signal moves the end effector
volatile int count = 0; // Counter to change direction
@@ -235,21 +234,21 @@
{
double filteredAbs_temp_r;
- if((check_calibration_l == 1) && (check_calibration_r == 1))
+ if((check_calibration_l == 1) && (check_calibration_r == 1)) // Check if EMG is calibrated
{
- for(int i = 0; i<250; i++)
+ for(int i = 0; i<250; i++) // Take samples of EMG
{
- filter_r();
+ filter_r(); // Filter signal
filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r;
wait(0.0004);
}
- filteredAbs_temp_r = filteredAbs_temp_r/250;
- if(filteredAbs_temp_r<=0.3) //if signal is lower then 0.5 the blue light goes on
+ filteredAbs_temp_r = filteredAbs_temp_r/250; // Take mean of signal
+ if(filteredAbs_temp_r <= 0.3) // If signal is lower than threshold, arm is not active
{
- led1.write(1); //led 1 is rood en uit
+ led1.write(1);
active_r = false;
}
- else if(filteredAbs_temp_r > 0.3) //if signal does not pass threshold value, blue light goes on
+ else if(filteredAbs_temp_r > 0.3) // If signal is higher than threshold, arm is active
{
led1.write(0);
active_r = true;
@@ -271,12 +270,12 @@
wait(0.0004);
}
filteredAbs_temp_l = filteredAbs_temp_l/250;
- if(filteredAbs_temp_l <= 0.3) //if signal is lower then 0.5 the blue light goes on
+ if(filteredAbs_temp_l <= 0.3)
{
led2.write(1);
active_l = false;
}
- else if(filteredAbs_temp_l > 0.3) //if signal does not pass threshold value, blue light goes on
+ else if(filteredAbs_temp_l > 0.3)
{
led2.write(0);
active_l = true;
@@ -290,23 +289,21 @@
led3.write(0);
double signal_collection_r = 0;
- for(int n =0; n < 5000; n++) //read for 5000 samples as calibration
+ for(int n =0; n < 5000; n++) // Take samples of EMG signal
{
- filter_r();
- emg_value_r = emg_r.read();
- emgFiltered_r = bqc.step( emg_value_r );
- filteredAbs_r = abs(emgFiltered_r);
-
+ emg_value_r = emg_r.read(); // Read EMG signal
+ emgFiltered_r = bqc.step(emg_value_r); // Filter signal
+ filteredAbs_r = abs(emgFiltered_r); // Take absolute value
signal_collection_r = signal_collection_r + filteredAbs_r ;
wait(0.0004);
if (n == 4999)
{
- avg_emg_r = signal_collection_r / n;
+ avg_emg_r = signal_collection_r / n; // Take mean value
}
}
led3.write(1);
- check_calibration_r = 1;
+ check_calibration_r = 1; // Calibration of right arm is done
}
// Calibrate left arm
@@ -315,9 +312,8 @@
led3.write(0);
double signal_collection_l = 0;
- for(int n = 0; n < 5000; n++) //read for 5000 samples as calibration
+ for(int n = 0; n < 5000; n++)
{
- filter_l();
emg_value_l = emg_l.read();
emgFiltered_l = bqc.step(emg_value_l);
filteredAbs_l = abs(emgFiltered_l);
@@ -375,13 +371,11 @@
}
else if(ydir) // Control in y-direction
{
- if(active_r && count == 0 &&
- reference2 < motor2Max ) //&& reference1 > motor2Min)
+ if(active_r && count == 0 && reference2 < motor2Max )
{
yVelocity = velocity;
}
- else if(active_l && count == 0
- && reference2 > motor2Min ) //&& reference1 > motor2Min)
+ else if(active_l && count == 0 && reference2 > motor2Min )
{
yVelocity = -velocity;
}
@@ -460,15 +454,6 @@
if(reference2 + msh[1]*processTs > motor2Max) reference2 = motor2Max;
else if(reference2 + msh[1]*processTs < motor2Min) reference2 = motor2Min;
else reference2 = reference2 + msh[1]*processTs;
-
- /*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);
}
}
@@ -488,7 +473,7 @@
stateChanged = false;
}
- // Continue button
+ // Calibration button
if(!button1)
{
currentState = CALIBRATING;
@@ -546,12 +531,10 @@
led4.write(1);
led5.write(0);
- pc.printf("START \r\n");
-
bqc.add(&bq1_low ).add(&bq2_high ).add(&bq3_notch ); // Make BiQuad Chain
processTicker.attach(&JointVelocities, processTs); // Ticker to process EMG
- controllerTicker.attach(&MotorController, controllerTs); // Ticker to control motor 1 (PID)
+ controllerTicker.attach(&MotorController, controllerTs); // Ticker to control motors
emgRight.attach(&check_emg_r, emgTs); // Ticker to sample EMG of right arm
emgLeft.attach(&check_emg_l, emgTs); // Ticker to sample EMG of left arm