Sem Bertelink
lololololol
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 26:088e397ec26f
- Parent:
- 24:a9ec9b836fd9
- Child:
- 27:d37b3a0e0f2b
diff -r a9ec9b836fd9 -r 088e397ec26f main.cpp
--- a/main.cpp Mon Oct 21 15:11:14 2019 +0000
+++ b/main.cpp Tue Oct 29 11:13:15 2019 +0000
@@ -7,20 +7,23 @@
// Pins
MODSERIAL pc(USBTX, USBRX);
-InterruptIn stepresponse(D0);
+
+QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
+QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
FastPWM PWM_motor_1(D6);
FastPWM PWM_motor_2(D5);
DigitalOut direction_motor_1(D7);
DigitalOut direction_motor_2(D4);
+
DigitalOut led_red(LED1);
DigitalOut led_green(LED2);
DigitalOut led_blue(LED3);
-AnalogIn emg_bl( A0 );
-AnalogIn emg_br( A1 );
-AnalogIn emg_leg( A2 );
+AnalogIn emg_bl(A0);
+AnalogIn emg_br(A1);
+AnalogIn emg_leg(A2);
InterruptIn button_1(SW2);
InterruptIn button_2(SW3);
@@ -33,45 +36,43 @@
Timer sinus_time;
Timeout rest_timeout;
Timeout mvc_timeout;
+Timeout led_timeout;
enum states {start, motor_calibration, demo_mode, emg_calibration, vertical_movement, horizontal_movement};
states CurrentState = start;
bool StateChanged = true;
enum substates {rest_biceps_left, mvc_biceps_left, rest_biceps_right, mvc_biceps_right, rest_biceps_leg, mvc_biceps_leg};
substates CurrentSubstate = rest_biceps_left;
bool SubstateChanged = true;
-bool pressed_1 = false;
-bool pressed_2 = false;
+volatile bool pressed_1 = false;
+volatile bool pressed_2 = false;
HIDScope scope(3);
-QEI encoder_1(D10,D11,NC,8400,QEI::X4_ENCODING);
-QEI encoder_2(D12,D13,NC,8400,QEI::X4_ENCODING);
-volatile double theta_1;
+
+volatile float theta_1;
//volatile float theta_error_1;
volatile float theta_reference_1;
-volatile double theta_2;
+volatile float theta_2;
//volatile float theta_error_2;
volatile float theta_reference_2;
-float Ts = 0.001;
+float Ts = 0.01;
float Kp;
float Ki;
float Kd;
-BiQuadChain highnotch;
-BiQuadChain low;
-BiQuad Lowpass1 (3.73938e-07, 7.47876e-07, 3.73938e-07, -1.90886e+00, 9.11279e-01);
-BiQuad Lowpass2 (1.00000e+00, 2.00000e+00, 1.00000e+00, -1.95979e+00, 9.62270e-01);
-BiQuad Highpass (9.69531e-01, -9.69531e-01, 0.00000e+00, -9.39063e-01, 0.00000e+00 );
-BiQuad Notch (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96863e-01);
+
+BiQuad Lowpass ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
+BiQuad Highpass ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
+BiQuad notch (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01);
+
int n = 0;
-double emgFiltered_bl;
-double emgFiltered_br;
-double emgFiltered_leg;
-double emg;
-double xmvc_value = 1e-11;
+float emgFiltered_bl;
+float emgFiltered_br;
+float emgFiltered_leg;
+float emg;
+float xmvc_value = 1e-11;
-int muscle;
float sum = 0;
-float rest_value;
+float xrest_value;
float rest_value_bl;
float rest_value_br;
float rest_value_leg;
@@ -81,6 +82,56 @@
float mvc_value_leg;
// functies
+void ledred()
+{
+ led_red = 0;
+ led_green = 1;
+ led_blue = 1;
+}
+void ledgreen()
+{
+ led_green=0;
+ led_blue=1;
+ led_red=1;
+}
+void ledblue()
+{
+ led_green=1;
+ led_blue=0;
+ led_red=1;
+}
+void ledyellow()
+{
+ led_green=0;
+ led_blue=1;
+ led_red=0;
+}
+void ledmagenta()
+{
+ led_green=1;
+ led_blue=0;
+ led_red=0;
+}
+void ledcyan()
+{
+ led_green=0;
+ led_blue=0;
+ led_red=1;
+}
+void ledwhite()
+{
+ led_green=0;
+ led_blue=0;
+ led_red=0;
+}
+void ledoff()
+{
+ led_green=1;
+ led_blue=1;
+ led_red=1;
+}
+
+
float CalculateError(float theta_reference,float theta)
{
float theta_error = theta_reference-theta;
@@ -141,7 +192,7 @@
void MotorControl()
{
ReadEncoder();
- theta_reference_1 = 360*sin(0.1f*sinus_time.read()*2*3.14f); // voor test, moet weg in eindscript
+ theta_reference_1 = 360.0f*sin(0.1f*sinus_time.read()*2.0f*3.14f); // voor test, moet weg in eindscript
CalculateDirectionMotor();
PWM_motor_1.write(fabs(Controller(CalculateError(theta_reference_1,theta_1),0)/360.0f));
PWM_motor_2.write(fabs(Controller(CalculateError(theta_reference_2,theta_2),1)/360.0f));
@@ -157,7 +208,7 @@
pressed_2 = !pressed_2;
}
-float EmgCalibration(double emgFiltered, float mvc_value, float rest_value)
+float EmgCalibration(float emgFiltered, float mvc_value, float rest_value)
{
float emgCalibrated;
if (emgFiltered <= rest_value) {
@@ -173,139 +224,198 @@
void emgsample()
{
- emgFiltered_bl = highnotch.step(emg_bl.read());
+ emgFiltered_bl = Highpass.step(emg_bl.read());
+ emgFiltered_bl = notch.step(emgFiltered_bl);
emgFiltered_bl = fabs(emgFiltered_bl);
- emgFiltered_bl = low.step(emgFiltered_bl);
+ emgFiltered_bl = Lowpass.step(emgFiltered_bl);
- emgFiltered_br = highnotch.step(emg_br.read());
+ emgFiltered_br = Highpass.step(emg_br.read());
+ emgFiltered_br = notch.step(emgFiltered_br);
emgFiltered_br = fabs(emgFiltered_br);
- emgFiltered_br = low.step(emgFiltered_br);
+ emgFiltered_br = Lowpass.step(emgFiltered_br);
- emgFiltered_leg = highnotch.step(emg_leg.read());
+ emgFiltered_leg = Highpass.step(emg_leg.read());
+ emgFiltered_leg = notch.step(emgFiltered_leg);
emgFiltered_leg = fabs(emgFiltered_leg);
- emgFiltered_leg = low.step(emgFiltered_leg);
+ emgFiltered_leg = Lowpass.step(emgFiltered_leg);
}
void rest()
{
- if (muscle == 0)
- {
+ if (CurrentSubstate == rest_biceps_left) {
emg = emgFiltered_bl;
+ //pc.printf("emg: %f \n\r",emgFiltered_bl);
}
- if (muscle == 2)
- {
+ if (CurrentSubstate == rest_biceps_right) {
emg = emgFiltered_br;
}
- if (muscle == 4)
- {
+ if (CurrentSubstate == rest_biceps_leg) {
emg = emgFiltered_leg;
}
- if (n < 50)
- {
+ if (n < 500) {
+ ledred();
sum = sum + emg;
+ //pc.printf("sum: %f \n\r",sum);
n++;
- rest_value = float (sum/n);
- rest_timeout.attach(rest,0.01f);
+ rest_timeout.attach(rest,0.001f);
}
- if (n == 50)
- {
+ if (n == 500) {
sum = sum + emg;
+ //pc.printf("sum: %f \n\r",sum);
n++;
- rest_value = float (sum/n);
- n = 0;
- sum = 0;
- if (muscle == 0)
- {
- rest_value_bl = rest_value;
+ xrest_value = float (sum/n);
+ if (CurrentSubstate == rest_biceps_left) {
+ rest_value_bl = xrest_value;
+ pc.printf("rest_value_bl %f \n\r", rest_value_bl);
CurrentSubstate = mvc_biceps_left;
SubstateChanged = true;
- led_red = 1;
+ ledblue();
+
}
- if (muscle == 2)
- {
- rest_value_br = rest_value;
+ if (CurrentSubstate == rest_biceps_right) {
+ rest_value_br = xrest_value;
+ pc.printf("rest_value_br %f \n\r", rest_value_br);
CurrentSubstate = mvc_biceps_right;
SubstateChanged = true;
- led_red = 1;
+ ledmagenta();
}
- if (muscle == 4)
- {
- rest_value_leg = rest_value;
+ if (CurrentSubstate == rest_biceps_leg) {
+ rest_value_leg = xrest_value;
+ pc.printf("rest_value_leg %f \n\r", rest_value_leg);
+ pc.printf("rest_value_bl %f \n\r", rest_value_bl);
CurrentSubstate = mvc_biceps_leg;
SubstateChanged = true;
- led_red = 1;
+ ledwhite();
}
}
}
void mvc()
{
- if (muscle == 1)
- {
+ if (CurrentSubstate == mvc_biceps_left) {
emg = emgFiltered_bl;
}
- if (muscle == 3)
- {
+ if (CurrentSubstate == mvc_biceps_right) {
emg = emgFiltered_br;
}
- if (muscle == 5)
- {
+ if (CurrentSubstate == mvc_biceps_leg) {
emg = emgFiltered_leg;
}
- if (emg >= xmvc_value)
- {
+ if (emg >= xmvc_value) {
xmvc_value = emg;
- }
+ }
n++;
- if (n < 100)
- {
- mvc_timeout.attach(mvc,0.01f);
+ if (n < 1000) {
+ mvc_timeout.attach(mvc,0.001f);
+ ledred();
}
- if (n == 100)
- {
- n = 0;
- if (muscle == 1)
- {
+ if (n == 1000) {
+ if (CurrentSubstate == mvc_biceps_left) {
mvc_value_bl = xmvc_value;
+ pc.printf("mvc_value_bl %f \n\r", mvc_value_bl);
CurrentSubstate = rest_biceps_right;
SubstateChanged = true;
- led_red = 1;
+ ledyellow();
}
- if (muscle == 3)
- {
+ if (CurrentSubstate == mvc_biceps_right) {
mvc_value_br = xmvc_value;
+ pc.printf("mvc_value_br %f \n\r", mvc_value_br);
CurrentSubstate = rest_biceps_leg;
SubstateChanged = true;
- led_red = 1;
+ ledcyan();
}
- if (muscle == 5)
- {
+ if (CurrentSubstate == mvc_biceps_leg) {
mvc_value_leg = xmvc_value;
+ pc.printf("mvc_value_leg %f \n\r", mvc_value_leg);
CurrentState = vertical_movement;
StateChanged = true;
- led_red = 1;
+ ledoff();
}
xmvc_value = 1e-11;
- led_red = 1;
}
}
+float emgCalibrated_bl;
+float emgCalibrated_br;
+float emgCalibrated_leg;
+
void WriteScope()
{
- //scope.set(0,theta_1);
- // scope.set(1,CalculateError(theta_reference_1,theta_1));
- // scope.set(2,theta_reference_1);
+ emgsample();
+ /*
+ if (emgFiltered_bl <= rest_value_bl) {
+ emgCalibrated_bl = 0;
+ }
+ if (emgFiltered_bl >= mvc_value_bl) {
+ emgCalibrated_bl = 1;
+ } else {
+ emgCalibrated_bl = (emgFiltered_bl - rest_value_bl) / (mvc_value_bl - rest_value_bl);
+ }
+ if (emgFiltered_br <= rest_value_br) {
+ emgCalibrated_br = 0;
+ }
+ if (emgFiltered_br >= mvc_value_br) {
+ emgCalibrated_br = 1;
+ } else {
+ emgCalibrated_br = (emgFiltered_br - rest_value_br) / (mvc_value_br - rest_value_br);
+ }
+ if (emgFiltered_leg <= rest_value_leg) {
+ emgCalibrated_leg = 0;
+ }
+ if (emgFiltered_leg >= mvc_value_leg) {
+ emgCalibrated_leg = 1;
+ } else {
+ emgCalibrated_leg = (emgFiltered_leg - rest_value_leg) / (mvc_value_leg - rest_value_leg);
+ }
+ */
+ /*
scope.set(0, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
- scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br) );
+ scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br));
scope.set(2, EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg));
+ */
+ /*
+ scope.set(0, emgCalibrated_bl);
+ scope.set(1, emgCalibrated_br);
+ scope.set(2, emgCalibrated_leg);
+ */
+ scope.set(0, emg_bl.read());
+ scope.set(1, emgCalibrated_bl);
+ scope.set(2, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
scope.send();
}
void SubstateTransition()
{
- pressed_1 = false;
- pressed_2 = false;
- SubstateChanged = false;
+ if (SubstateChanged == true) {
+ SubstateChanged = false;
+ pressed_1 = false;
+ pressed_2 = false;
+ if (CurrentSubstate == rest_biceps_left) {
+ ledgreen();
+ pc.printf("groen \n\r");
+ pc.printf("Initiating rest_biceps_left\n\r");
+ }
+ if (CurrentSubstate == mvc_biceps_left) {
+ //ledblue();
+ pc.printf("Initiating mvc_biceps_left\n\r");
+ }
+ if (CurrentSubstate == rest_biceps_right) {
+ //ledyellow();
+ pc.printf("Initiating rest_biceps_right\n\r");
+ }
+ if (CurrentSubstate == mvc_biceps_right) {
+ //ledmagenta();
+ pc.printf("Initiating mvc_biceps_right\n\r");
+ }
+ if (CurrentSubstate == rest_biceps_leg) {
+ //ledcyan();
+ pc.printf("Initiating rest_biceps_leg\n\r");
+ }
+ if (CurrentSubstate == mvc_biceps_leg) {
+ //ledwhite();
+ pc.printf("Initiating mvc_biceps_leg\n\r");
+ }
+ }
}
void while_start()
@@ -331,7 +441,7 @@
void while_demo_mode()
{
// Do demo mode stuff
- if (pressed_1 || pressed_2) {
+ if ((pressed_1) || (pressed_2)) {
CurrentState = emg_calibration;
StateChanged = true;
}
@@ -343,72 +453,58 @@
switch (CurrentSubstate) {
case rest_biceps_left:
SubstateTransition();
- muscle = 0;
- led_green = 0;
- if (pressed_1 || pressed_2)
- {
- led_green = 1;
- led_red = 0;
+ if ((pressed_1) || (pressed_2)) {
+ pressed_1 = false;
+ pressed_2 = false;
+ n = 0;
+ sum = 0;
rest();
}
break;
case mvc_biceps_left:
SubstateTransition();
- muscle = 1;
- led_blue = 0;
- if (pressed_1 || pressed_2)
- {
- led_blue = 1;
- led_red = 0;
+ if ((pressed_1) || (pressed_2)) {
+ pressed_1 = false;
+ pressed_2 = false;
+ n = 0;
mvc();
}
break;
case rest_biceps_right:
SubstateTransition();
- muscle = 2;
- led_red = 0;
- led_green = 0;
- if (pressed_1 || pressed_2)
- {
- led_green = 1;
+ if ((pressed_1) || (pressed_2)) {
+ pressed_1 = false;
+ pressed_2 = false;
+ n = 0;
+ sum = 0;
rest();
}
break;
case mvc_biceps_right:
SubstateTransition();
- muscle = 3;
- led_red = 0;
- led_blue = 0;
- if (pressed_1 || pressed_2)
- {
- led_blue = 1;
+ if ((pressed_1) || (pressed_2)) {
+ pressed_1 = false;
+ pressed_2 = false;
+ n = 0;
mvc();
}
break;
case rest_biceps_leg:
SubstateTransition();
- muscle = 4;
- led_green = 0;
- led_blue = 0;
- if (pressed_1 || pressed_2)
- {
- led_green = 1;
- led_blue = 1;
- led_red = 0;
+ if ((pressed_1) || (pressed_2)) {
+ pressed_1 = false;
+ pressed_2 = false;
+ n = 0;
+ sum = 0;
rest();
}
break;
case mvc_biceps_leg:
SubstateTransition();
- muscle = 5;
- led_red = 0;
- led_green = 0;
- led_blue = 0;
- if (pressed_1 || pressed_2)
- {
- led_green = 1;
- led_blue = 1;
- led_red = 0;
+ if ((pressed_1) || (pressed_2)) {
+ pressed_1 = false;
+ pressed_2 = false;
+ n = 0;
mvc();
}
break;
@@ -420,7 +516,7 @@
void while_vertical_movement()
{
// Do vertical movement stuff
- if (pressed_1 || pressed_2) { // EMG gebaseerde threshold aanmaken
+ if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken
CurrentState = horizontal_movement;
StateChanged = true;
}
@@ -429,7 +525,7 @@
void while_horizontal_movement()
{
// Do horizontal movement stuff
- if (pressed_1 || pressed_2) { // EMG gebaseerde threshold aanmaken
+ if ((pressed_1) || (pressed_2)) { // EMG gebaseerde threshold aanmaken
CurrentState = vertical_movement;
StateChanged = true;
}
@@ -437,9 +533,9 @@
void StateTransition()
{
- pressed_1 = false;
- pressed_2 = false;
if (StateChanged) {
+ pressed_1 = false;
+ pressed_2 = false;
if (CurrentState == start) {
pc.printf("Initiating start.\n\r");
}
@@ -499,14 +595,15 @@
{
pc.baud(115200);
pc.printf("Hello World!\n\r");
+ ledoff();
button_1.fall(go_next_1);
button_2.fall(go_next_2);
- sinus_time.start();
- PWM_motor_1.period_ms(10);
- motor_control.attach(&MotorControl, Ts);
- write_scope.attach(&WriteScope, 0.1);
+ //sinus_time.start();
+ //PWM_motor_1.period_ms(10);
+ //motor_control.attach(&MotorControl, Ts);
+ write_scope.attach(&WriteScope, 0.001);
//TickerStateMachine.attach(StateMachine,1.00f);
while(true) {
StateMachine();
}
-}
\ No newline at end of file
+}