Mart Wijntjes
sdf
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM
Diff: Opzet_Eli.cpp
- Revision:
- 3:0c31a4a5d1fe
- Parent:
- 2:5730195cf595
--- a/Opzet_Eli.cpp Mon Oct 14 12:14:18 2019 +0000
+++ b/Opzet_Eli.cpp Fri Oct 18 12:06:15 2019 +0000
@@ -3,22 +3,263 @@
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
+#include "biquadFilter.h"
+#include "math.h"
+#include "FastPWM.h"
// Define objects
+AnalogIn emg1(A0);
+AnalogIn emg2(A1);
+AnalogIn emg3(A2);
+AnalogIn emg4(A3);
MODSERIAL pc(USBTX,USBRX);
DigitalOut led_red(LED_RED);
DigitalOut led_blue(LED_BLUE);
DigitalOut led_green(LED_GREEN);
InterruptIn button_Mbed(PTC6); //Button 1 Mbed
-InterruptIn button_1(PTB10); //Button 2 BRS
-InterruptIn button_2(PTB11); // Button 3 BRS
+InterruptIn button_1(); //Button 2 BRS
+InterruptIn button_2(); // Button 3 BRS
+
+FastPWM motor1(D6);
+DigitalOut motor1_dir(D7);
+FastPWM motor2(D5);
+DigitalOut motor2_dir(D4);
+
+
+//ticker setup
+Ticker states_machine;
+
+
+//encoder setup
+QEI encoder_1(D13,D12,NC,32,QEI::X4_ENCODING);
+QEI encoder_2(D11,D10,NC,32,QEI::X4_ENCODING);
+
+//motor setup
+int motordir1 = 1;
+int motordir2 = 1;
+double Kp = 17.5;
+
+//Hidscope setup
+HIDScope scope(4);
+
+// EMG setup
+biquadFilter filterhigh1(-1.1430, 0.4128, 0.6389, -1.2779, 0.6389);
+biquadFilter filterlow1(1.9556, 0.9565, 0.9780, 1.9561, 0.9780);
+biquadFilter notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
+biquadFilter filterlow2(-1.9645, 0.9651, 1.5515e-4, 3.1030e-4, 1.5515e-4);
+double emg_value_1;
+double signalpart1_1;
+double signalpart2_1;
+double signalpart3_1;
+double signalpart4_1;
+double signalfinal_1;
+double emgsignal_1;
+double onoffsignal_1;
+double maxcal_1=0;
-// Functions
+double emg_value_2;
+double signalpart1_2;
+double signalpart2_2;
+double signalpart3_2;
+double signalpart4_2;
+double signalfinal_2;
+double emgsignal_2;
+double onoffsignal_2;
+double maxcal_2=0;
+
+double emg_value_3;
+double signalpart1_3;
+double signalpart2_3;
+double signalpart3_3;
+double signalpart4_3;
+double signalfinal_3;
+double emgsignal_3;
+double onoffsignal_3;
+double maxcal_3=0;
+
+double emg_value_4;
+double signalpart1_4;
+double signalpart2_4;
+double signalpart3_4;
+double signalpart4_4;
+double signalfinal_4;
+double emgsignal_4;
+double onoffsignal_4;
+double maxcal_4=0;
+
+double emgx;
+double emgy;
+double kemg = 0.1;
+
+//kinematics setup
+double xgoal;
+double ygoal;
+double theta1;
+double theta2;
+
+
+// State machine
enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};
-
states CurrentState = START;
bool StateChanged = true; // this is the initialization of the first state
+// button functions
+
+void emgread(){
+ emg_value_1 = emg1.read();//read the emg value from the elektrodes
+ signalpart1_1 = notch.step(emg_value_1);//Highpass filter for removing offset and artifacts
+ signalpart2_1 = filterhigh1.step(signalpart1_1);//rectify the filtered signal
+ signalpart3_1 = abs(signalpart2_1);//low pass filter to envelope the emg
+ signalpart4_1 = filterlow1.step(signalpart3_1);//notch filter to remove 50Hz signal
+ emgsignal_1 = filterlow2.step(signalpart4_1);//2nd low pass filter to envelope the emg
+ emg_value_2 = emg2.read();//read the emg value from the elektrodes
+ signalpart1_2 = notch.step(emg_value_2);//Highpass filter for removing offset and artifacts
+ signalpart2_2 = filterhigh1.step(signalpart1_2);//rectify the filtered signal
+ signalpart3_2 = abs(signalpart2_2);//low pass filter to envelope the emg
+ signalpart4_2 = filterlow1.step(signalpart3_2);//notch filter to remove 50Hz signal
+ emgsignal_2 = filterlow2.step(signalpart4_2);//2nd low pass filter to envelope the emg
+ emg_value_3 = emg3.read();//read the emg value from the elektrodes
+ signalpart1_3 = notch.step(emg_value_3);//Highpass filter for removing offset and artifacts
+ signalpart2_3 = filterhigh1.step(signalpart1_3);//rectify the filtered signal
+ signalpart3_3 = abs(signalpart2_3);//low pass filter to envelope the emg
+ signalpart4_3 = filterlow1.step(signalpart3_3);//notch filter to remove 50Hz signal
+ emgsignal_3 = filterlow2.step(signalpart4_3);//2nd low pass filter to envelope the emg
+ emg_value_4 = emg4.read();//read the emg value from the elektrodes
+ signalpart1_4 = notch.step(emg_value_4);//Highpass filter for removing offset and artifacts
+ signalpart2_4 = filterhigh1.step(signalpart1_4);//rectify the filtered signal
+ signalpart3_4 = abs(signalpart2_4);//low pass filter to envelope the emg
+ signalpart4_4 = filterlow1.step(signalpart3_4);//notch filter to remove 50Hz signal
+ emgsignal_4 = filterlow2.step(signalpart4_4);//2nd low pass filter to envelope the emg
+}
+void emgcal(){
+ emgread();
+ double signalmeasure_1 = emgsignal_1;
+ if (signalmeasure_1 > maxcal_1){//determine what the highest reachable emg signal is
+ maxcal_1 = signalmeasure_1;}
+ double signalmeasure_2 = emgsignal_2;
+ if (signalmeasure_2 > maxcal_2){//determine what the highest reachable emg signal is
+ maxcal_2 = signalmeasure_2;}
+ double signalmeasure_3 = emgsignal_3;
+ if (signalmeasure_3 > maxcal_3){//determine what the highest reachable emg signal is
+ maxcal_3 = signalmeasure_3;}
+ double signalmeasure_4 = emgsignal_4;
+ if (signalmeasure_4 > maxcal_4){//determine what the highest reachable emg signal is
+ maxcal_4 = signalmeasure_4;}
+ scope.set(0,signalmeasure_1);//set emg signal to scope in channel 1
+ scope.set(1,signalmeasure_2);//set filtered signal to scope in channel 2
+ scope.set(2,signalmeasure_3);//set filtered signal to scope in channel 3
+ scope.set(3,signalmeasure_4);//set filtered signal to scope in channel 4
+ }
+
+
+void emgshow(){
+ emgread();
+ onoffsignal_1=emgsignal_1/maxcal_1;// emg positive x
+ onoffsignal_2=emgsignal_2/maxcal_2;// emg negative x
+ onoffsignal_3=emgsignal_3/maxcal_3;// emg positive y
+ onoffsignal_4=emgsignal_4/maxcal_4;// emg negative y
+ scope.set(0,onoffsignal_1);//set emg signal to scope in channel 1
+ scope.set(1,onoffsignal_2);//set filtered signal to scope in channel 2
+ scope.set(2,onoffsignal_3);//set filtered signal to scope in channel 3
+ scope.set(3,onoffsignal_4);//set filtered signal to scope in channel 4
+ emgx = onoffsignal_1- onoffsignal_2;
+ emgy = onoffsignal_3- onoffsignal_4;
+ scope.send();
+ }
+
+void forwardkin(){
+ double qref2 = encoder_2.getPulses();
+ double q2_correct = (qref2*2*3.14)/8400.0;
+ double qref1 = encoder_1.getPulses();
+ double q1_correct = (qref1*2*3.14)/8400.0;
+ double x = 0.3*cos(q1_correct)+0.3*cos(q1_correct+q2_correct);
+ double y = 0.3*sin(q1_correct)+0.3*cos(q1_correct+q2_correct);
+ emgshow();
+ xgoal = x + emgx* kemg;
+ ygoal = y + emgy* kemg;
+ }
+
+void reversekin(){
+ forwardkin();
+ theta2= acos(((xgoal*xgoal)+(ygoal*ygoal)-(0.3*0.3)-(0.3*0.3))/(2*0.3*0.3));
+ theta1= atan(ygoal/xgoal)-atan((0.3*sin(theta2))/(0.3+0.3*cos(theta2)));
+ }
+
+void motor_position()
+{
+ reversekin();
+ double pos_1 = encoder_1.getPulses();
+ double poscorrect_1 = (pos_1*3.14*2)/8400.0;
+ double error1 = theta1-poscorrect_1;
+ if (error1 >=0) motor1_dir=1;
+ else motor1_dir=0;
+ if (fabs(error1)>1) motor1 = 1;
+ else motor1 = fabs(error1);
+
+ double pos_2 = encoder_2.getPulses();
+ double poscorrect_2 = (pos_2*3.14*2)/8400.0;
+ double error2 = theta2-poscorrect_2;
+ if (error2 >=0) motor2_dir=1;
+ else motor1_dir=0;
+ if (fabs(error2)>1) motor2 = 1;
+ else motor1 = fabs(error2);
+ }
+
+void kalmot(){
+ CurrentState = KAL_ME;
+ StateChanged = true;
+}
+
+void kalemg(){
+ CurrentState = KAL_EMG;
+ StateChanged = true;
+ wait(0.2f);
+}
+void movestart(){
+ CurrentState = MOVE_START;
+ StateChanged = true;
+ wait(0.2f);
+ }
+
+void readystart(){
+ CurrentState = READY_START;
+ StateChanged = true;
+ wait(0.2f);
+ }
+void demo(){
+ CurrentState = DEMO;
+ StateChanged = true;
+ wait(0.2f);
+ }
+void move(){
+ CurrentState = MOVE;
+ StateChanged = true;
+ wait(0.2f);
+ }
+void wait(){
+ CurrentState = WAIT;
+ StateChanged = true;
+ wait(0.2f);
+ }
+void off(){
+ CurrentState = OFF;
+ StateChanged = true;
+ wait(0.2f);
+ }
+
+//led functions
+void flashred(){
+ led_red = !led_red;
+ wait(0.4f);
+ }
+void flashgreen(){
+ led_green = !led_green;
+ wait(0.4f);
+ }
+void flashblue(){
+ led_blue = !led_blue;
+ wait(0.4f);
+ }
// Function START_TO_KAL_ME
void StateMachine(void)
@@ -34,21 +275,16 @@
led_green = 1;
StateChanged = false;
}
- if (button_Mbed.mode (PullDown)== false; ) // State switches when button pressed
- {
- CurrentState = KAL_ME;
- StateChanged = true;
- wait(0.2f);
-
- }
+ button_Mbed.rise(&kalmot); // State switches when button pressed
+
break; // end of state START
case KAL_ME:
if (StateChanged)
{
pc.printf("Calibration ME state, red ld flickers slow");
- //FUNCTION Red led flickers slow
-
+ flashred();
+
// FUNCTION Move to mechanical stop, include v_motor, t_passed
// FUNCTION Reset encoders
@@ -56,28 +292,19 @@
StateChanged = false;
}
- if (v_motor == 0 && t_passed > 2) // FUNCTION t_passed, included in v_motor?
- {
- CurrentState = KAL_EMG;
- StateChanged = true;
- }
+ button_Mbed.rise(&kalemg);
break; // end of state KAL_ME
case KAL_EMG:
if (StateChanged)
{
- // FUCNTION Red led flickers fast
-
- //FUNCTION Measure EMG_max, EMG variable meten, t_passed
-
+ flashgreen();
+ emgcal();
StateChanged = false;
}
- if (EMG < 0.1*EMG_max && t_passed > 2)
- {
- CurrentState = MOVE_START;
- StateChanged = true;
- }
+ button_Mbed.rise(&readystart);
+
break; // end of state KAL_EMG
case MOVE_START:
@@ -92,11 +319,8 @@
StateChanged = false;
}
- if (current_position == start_position && t_passed > 2) // FUNCTIO t_passed
- {
- CurrentState = READY_START;
- StateChanged = true;
- }
+ button_Mbed.rise(&readystart);
+
break; // end of state MOVE_START
case READY_START:
@@ -109,18 +333,9 @@
StateChanged = false;
}
- if (button_1.read() == false) // Button 1
- {
- CurrentState = DEMO;
- StateChanged = true;
- wait(0.2f);
- }
- else if (button_2.read() == false || EMG > 0.2*EMG_max) // Button 2 or 20% EMG_max
- {
- CurrentState = MOVE;
- StateChanged = true;
- wait(0.2f);
- }
+ button_Mbed.rise(&demo);
+ //button_1.rise(&move);
+
break; // end of state READY_START
case DEMO:
@@ -133,11 +348,7 @@
StateChanged = false;
}
- if (current_position == end_position && t_passed > 2)
- {
- CurrentState = MOVE_START;
- StateChanged = true;
- }
+ button_Mbed.rise(&movestart);
break; // end of state DEMO
case MOVE:
@@ -152,19 +363,9 @@
StateChanged = false;
}
- if (button_Mbed.read() == false)
- {
- CurrentState = OFF;
- StateChanged = true;
- wait(0.2f);
- }
+ button_Mbed.rise(&off);
+ button1.rise(&wait);
- else if (button_1.read() == false || EMG < 0.2*EMG_max)
- {
- CurrentState = WAIT;
- StateChanged = true;
- wait(0.2f);
- }
break; // end of state MOVE
case WAIT:
@@ -174,24 +375,10 @@
led_blue = 0;
led_green = 1
- if (button_Mbed.read() == false) // For five second, too hard
- {
- CurrentState = OFF;
- StateChanged = true;
- wait(0.2f);
- )
- else if (button_1.read() == false)
- {
- CurrentState = MOVE_START;
- StateChanged = true;
- wait(0.2f);
- }
- else if (button_2.read() == false || EMG > 0.2*EMG_max)
- {
- CurrentState = MOVE;
- StateChanged = true;
- wait(0.2f);
- }
+ button_Mbed.rise(&off);
+ button1.rise(&movestart);
+ button2.rise(&move);
+
break; // end of state WAIT
@@ -210,11 +397,11 @@
int main(void) // wat hier in moet snap ik nog niet
{
+ states_machine.attach(&StateMachine, 0.002)
// hier moeten dingen komen
while (true)
{
CheckForCommandFromTerminal();
- StateMachine();
}
}