changed the location of all constants to Constants.h
Dependencies: HIDScope QEI biquadFilter mbed
Fork of
State_machine
by 
Casper Kroon
Diff: main.cpp
- Revision:
- 4:dfe39188f2b2
- Parent:
- 3:ed4676f76a5c
- Child:
- 5:e96d03bd557c
--- a/main.cpp Wed Oct 31 10:35:27 2018 +0000
+++ b/main.cpp Thu Nov 01 14:11:05 2018 +0000
@@ -1,7 +1,7 @@
#include "mbed.h"
#include "QEI.h"
#include "HIDScope.h"
-#include "MODSERIAL.h"
+//#include "MODSERIAL.h"
#include "BiQuad.h"
#include "math.h"
@@ -23,9 +23,7 @@
BiQuad emg0bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
BiQuad emg0bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
BiQuad emg0bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
-BiQuadChain emg0bqc1; // merged chain of three filters
-BiQuadChain emg0bqc2;
-BiQuadChain emg0bqc3;
+BiQuadChain emg0bqc; // merged chain of three filters
AnalogIn emg1( A1 ); // EMG at A1
BiQuad emg1bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
@@ -33,7 +31,6 @@
BiQuad emg1bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
BiQuadChain emg1bqc; // merged chain of three filters
-
AnalogIn emg2( A2 ); // EMG at A2
BiQuad emg2bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz Q at around 1
BiQuad emg2bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
@@ -43,8 +40,11 @@
DigitalIn kill_switch(SW2); //position has to be changed
DigitalIn next_switch(SW3); //name and position should be replaced
-enum states{PositionCalibration, EmgCalibration, Movement, KILL};
-states CurrentState;
+enum position_states{PositionCalibration, EmgCalibration, Movement, KILL};
+enum emg_states{EMG0, EMG1, EMG2};
+position_states CurrentState;
+emg_states CalibrationState;
+
Ticker sample_timer;
Ticker MotorsTicker;
Timer timer;
@@ -53,8 +53,8 @@
DigitalOut ledred(LED_RED);
DigitalOut ledgreen(LED_GREEN);
DigitalOut ledblue(LED_BLUE);
-MODSERIAL pc(USBTX, USBRX);
-HIDScope scope(5);
+//MODSERIAL pc(USBTX, USBRX);
+HIDScope scope(6);
bool emg0Bool = 0; // I don't know if these NEED to be global, but when I tried to put them in they wouldn't work...
int emg0Ignore = 0;
@@ -62,6 +62,9 @@
int emg1Ignore = 0;
bool emg2Bool = 0;
int emg2Ignore = 0;
+float Calibration0;
+float Calibration1;
+float Calibration2;
double input = 0; // raw input
double filtHigh = 0; // filtered after highpass
@@ -76,42 +79,115 @@
volatile float pwm_value1 = 0.0;
volatile float pwm_value2 = 0.0;
-/** Sample functions
- * these functions sample the emg and send it to HIDScope
- **/
-bool emg0Filter(void){
+void positionCalibration() {
+ while(!button1) {
+ directionpin1 = true;
+ pwm_value1 = 0.7f;
+ }
+ pwm_value1 = 0.0f;
+ while(!button2) {
+ directionpin2 = true;
+ pwm_value2 = 0.6f;
+ }
+ pwm_value2 = 0.0f;
+
+ if(!next_switch) {
+ CurrentState = EmgCalibration;
+ }
+}
+
+void CalibrateEMG0(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
+ ledred = !ledred;
+ int C = 500; // half a second at 1000Hz
+ double A0=0, A1=0, A2=0, A3=0, A4=0;
+ double emg0FiAbs;
+ while (C > 0){
+ emg0FiAbs = fabs( emg1bqc.step(emg1.read()));
+ if (C==500){ //first instance make all values the first in case this is the highest
+ A0=A1=A2=A3=A4=emg0FiAbs;
+ }
+ else if(emg0FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
+ A4=A3;
+ A3=A2;
+ A2=A1;
+ A1=A0;
+ A0=emg0FiAbs;
+ }
+ C--;
+ wait(0.001f);
+ }
+ Calibration0 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold
+ ledred = !ledred;
+}
- input = emg0.read();
- scope.set( 0, input);
- filtHigh = emg0bqc1.step(emg0.read());
- scope.set( 1, filtHigh);
- filtlow = emg0bqc2.step(emg0.read());
- scope.set( 2, filtlow);
- filtNotch = emg0bqc3.step(emg0.read());
- scope.set( 3, filtNotch);
-
+void CalibrateEMG1(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
+ ledgreen = !ledgreen;
+ int C = 500; // half a second at 1000Hz
+ double A0=0, A1=0, A2=0, A3=0, A4=0;
+ double emg1FiAbs;
+ while (C > 0){
+ emg1FiAbs = fabs( emg1bqc.step(emg1.read()));
+ if (C==500){ //first instance make all values the first in case this is the highest
+ A0=A1=A2=A3=A4=emg1FiAbs;
+ }
+ else if(emg1FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
+ A4=A3;
+ A3=A2;
+ A2=A1;
+ A1=A0;
+ A0=emg1FiAbs;
+ }
+ C--;
+ wait(0.001f);
+ }
+ Calibration1 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold
+ ledgreen = !ledgreen;
+}
+
+void CalibrateEMG2(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles.
+ ledblue = !ledblue;
+ int C = 500; // half a second at 1000Hz
+ double A0=0, A1=0, A2=0, A3=0, A4=0;
+ double emg2FiAbs;
+ while (C > 0){
+ emg2FiAbs = fabs( emg2bqc.step(emg2.read()));
+ if (C==500){ //first instance make all values the first in case this is the highest
+ A0=A1=A2=A3=A4=emg2FiAbs;
+ }
+ else if(emg2FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
+ A4=A3;
+ A3=A2;
+ A2=A1;
+ A1=A0;
+ A0=emg2FiAbs;
+ }
+ C--;
+ wait(0.001f);
+ }
+ Calibration2 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold
+ ledblue = !ledblue;
+}
+
+bool emg0Filter(void){
+ double emg0filteredAbs = fabs( emg0bqc.step(emg0.read())); // Filter and make absolute,
/* this is the threshhold */
- emg0filteredAbs = fabs(filtNotch);
- if (emg0filteredAbs > threshold0) { // when above threshold set bool to 1, here can the parameters be changed using global variables
+ if (emg0filteredAbs > Calibration0) { // when above threshold set bool to 1, here can the parameters be changed using global variables
emg0Bool = 1;
- emg0Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
+ emg0Ignore = IGNORECOUNT; // here is the counter reset to catch sudden jolts
}
else if (emg0Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
emg0Bool = 0;
}
else {
emg0Ignore--; // else decrease counter by one each time has passed without threshold being met
- }
-
- scope.set( 4, emg0Bool);
- scope.send();
+ }
return emg0Bool;
}
bool emg1Filter(void){
double emg1filteredAbs = fabs( emg1bqc.step(emg1.read())); // Filter and make absolute
/* this is the threshhold */
- if (emg1filteredAbs > threshold1) { // when above threshold set bool to 1 here can the parameters be changed using global variables
+ if (emg1filteredAbs > Calibration1) { // when above threshold set bool to 1 here can the parameters be changed using global variables
emg1Bool = true;
emg1Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
}
@@ -127,79 +203,71 @@
bool emg2Filter(void){
double emg2filteredAbs = fabs( emg2bqc.step(emg2.read())); // Filter and make absolute
/* this is the threshhold */
- if (emg2filteredAbs > threshold2) { // when above threshold set bool to 1 here can the parameters be changed using global variables
+ if (emg2filteredAbs > Calibration2) { // when above threshold set bool to 1 here can the parameters be changed using global variables
emg2Bool = true;
emg2Ignore = IGNORECOUNT; // here is the counter increased ( at 1000 Hz, this is 0.1 sec)
}
else if (emg2Ignore < 0){ // if the ignore-counter is down to zero, set the bool back to 0
- emg2Bool = false;
+ emg2Bool = false;
}
else {
emg2Ignore--; // else decrease counter by one each time has passed without threshold being met
}
+
return emg2Bool;
}
-void sample() {
- bool Bool1 = emg0Filter(); // whatever name casper uses for the bool
- // bool Bool2 = emg1Filter();
- // bool Bool3 = emg2Filter();
-}
-void positionCalibration() {
- while(!button1){
- directionpin1 = true;
- pwm_value1 = 0.7f;
- }
- pwm_value1 = 0.0f;
- while(!button2){
- directionpin2 = true;
- pwm_value2 = 0.7f;
- }
- pwm_value2 = 0.0f;
-
- // pwm_value1 = potmeter1;
- // pwm_value2 = potmeter2;
+void sample() {
+ emg0Filter();
+ emg1Filter();
+ emg2Filter();
- if (!next_switch) {
- CurrentState = EmgCalibration;
- pc.printf("current state = EmgCalibration\n\r");
- }
+ scope.set(0, Calibration0);
+ scope.set(1, Calibration1);
+ scope.set(2, Calibration2);
+ scope.set(3, emg0Bool);
+ scope.set(4, emg1Bool);
+ scope.set(5, emg2Bool);
+ scope.send();
}
-void emg0Calibration() {
- int C = 500; // half a second at 1000Hz
- double A0=0, A1=0, A2=0, A3=0, A4=0;
- double emg0FiAbs;
- while (C > 0){
- emg0FiAbs = fabs( emg1bqc.step(emg0.read()));
- if (C==500){ //first instance make all values the first in case this is the highest
- A0=A1=A2=A3=A4=emg0FiAbs;
+void emgCalibration() {
+ emg0bqc.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 ); // combining biquad chains is done in main, before the ticker, so it happens only once.
+ emg1bqc.add( &emg1bq1 ).add( &emg1bq2 ).add ( &emg1bq3 );
+ emg2bqc.add( &emg2bq1 ).add( &emg2bq2 ).add ( &emg2bq3 );
+
+ bool u = true;
+ CalibrationState = EMG0;
+ while (u){ // !!! this is a place holder for the calibration!!!
+ switch(CalibrationState) {
+ case EMG0:
+ if(!next_switch) {
+ CalibrateEMG0(); // calibration function
+ CalibrationState = EMG1;
+ }
+ break;
+ case EMG1:
+ if(!next_switch) {
+ CalibrateEMG1(); // calibration function
+ CalibrationState = EMG2;
+ }
+ break;
+ case EMG2:
+ if(!next_switch) {
+ CalibrateEMG2(); // calibration function
+ CurrentState = Movement;
+ sample_timer.attach(&sample, 0.001); //ticker at 1000Hz
+ ledred = false; // to indicate filter is working
+ ledgreen = false;
+ ledblue = false;
+ u = false;
+ }
+ break;
}
- else if(emg0FiAbs > A0){ // if there is a higher value change the inputs to be the highest 5
- A4=A3;
- A3=A2;
- A2=A1;
- A1=A0;
- A0=emg0FiAbs;
- }
- C--;
- wait(0.001f);
- threshold0 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold
- }
-
- if (!next_switch) {
- CurrentState = Movement;
- pc.printf("current state = Movement\n\r");
- }
+ }
}
-void emg1Calibration() {
-
-}
-void emg2Calibration() {
-
-}
void movement() {
@@ -212,8 +280,8 @@
int main()
{
- pc.baud(115200);
- pc.printf(" ** program reset **\n\r");
+// pc.baud(115200);
+// pc.printf(" ** program reset **\n\r");
pwmpin1.period_us(60);
pwmpin2.period_us(60);
directionpin1 = true;
@@ -222,73 +290,72 @@
ledred = true;
ledgreen = true;
ledblue = true;
-
- // emg filters
- // combining biquad chains is done in main, before the ticker, so only once.
- emg0bqc1.add( &emg0bq1 );
- emg0bqc2.add( &emg0bq1 ).add( &emg0bq2 );
- emg0bqc3.add( &emg0bq1 ).add( &emg0bq2 ).add ( &emg0bq3 ); // combining biquad chains is done in main, before the ticker, so only once.
-
+
MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz
- sample_timer.attach(&sample, 0.001); //ticker at 1000Hz
CurrentState = PositionCalibration;
- pc.printf("current state = PositionCalibration\n\r");
+// pc.printf("current state = PositionCalibration\n\r");
while (true) {
switch(CurrentState) {
case PositionCalibration:
- positionCalibration();
- if (!kill_switch) {
- CurrentState = KILL;
- pc.printf("current state = KILL\n\r");
- }
+ ledgreen = false;
+ positionCalibration();
+ if (!kill_switch) {
+ CurrentState = KILL;
+// pc.printf("current state = KILL\n\r");
+ }
break;
case EmgCalibration:
- emg0Calibration();
- //emg1Calibration();
- //emg2Calibration();
-
- if (!kill_switch) {
- CurrentState = KILL;
- pc.printf("current state = KILL\n\r");
- }
+ ledgreen = true;
+ ledblue = false;
+ emgCalibration();
+ //emg1Calibration();
+ //emg2Calibration();
+
+ if (!kill_switch) {
+ CurrentState = KILL;
+// pc.printf("current state = KILL\n\r");
+ }
break;
case Movement:
- movement();
-
- if (!kill_switch) {
- CurrentState = KILL;
- pc.printf("current state = KILL\n\r");
- }
+ ledred = true;
+ ledgreen = false;
+ ledblue = false;
+ movement();
+
+ if (!kill_switch) {
+ CurrentState = KILL;
+// pc.printf("current state = KILL\n\r");
+ }
break;
case KILL:
- bool u = true;
- ledgreen = true;
- ledblue = true;
- ledred = false;
-
- pwm_value1 = 0;
- pwm_value2 = 0;
-
- timer.start();
- if (timer.read_ms()> 2000) {
- timer.stop();
- timer.reset();
- while(u) {
- if (!kill_switch) {
- timer.start();
- u = false;
- ledred = true;
- CurrentState = PositionCalibration;
- pc.printf("current state = PositionCalibration\n\r");
- wait(1.0f);
- }
+ bool u = true;
+ ledgreen = true;
+ ledblue = true;
+ ledred = false;
+
+ pwm_value1 = 0;
+ pwm_value2 = 0;
+
+ timer.start();
+ if (timer.read_ms()> 2000) {
+ timer.stop();
+ timer.reset();
+ while(u) {
+ if (!kill_switch) {
+ timer.start();
+ u = false;
+ ledred = true;
+ CurrentState = PositionCalibration;
+// pc.printf("current state = PositionCalibration\n\r");
+ wait(1.0f);
}
}
+ }
break;
}
wait(0.2f);