Peter Knoben
Working EMG signals Mode control Direction control Position control Speed control
Dependencies: AnglePosition2 Encoder FastPWM HIDScope MODSERIAL Movement PIDController Servo SignalNumber2 biquadFilter mbed
Fork of
kinematics_control_copyfds
by 
Peter Knoben
Revision 4:e15fc329b88b, committed 2017-10-31
- Comitter:
- peterknoben
- Date:
- Tue Oct 31 14:35:52 2017 +0000
- Parent:
- 3:c768a37620c9
- Child:
- 5:b4abbd3c513c
- Commit message:
- sdfasdf
Changed in this revision
--- a/AnglePosition.lib Mon Oct 30 10:43:26 2017 +0000 +++ b/AnglePosition.lib Tue Oct 31 14:35:52 2017 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/peterknoben/code/AnglePosition/#d7e19af20f93 +https://os.mbed.com/users/peterknoben/code/AnglePosition2/#5c789825341d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Movement.lib Tue Oct 31 14:35:52 2017 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/peterknoben/code/Movement/#9c61094ffaac
--- a/SignalNumber.lib Mon Oct 30 10:43:26 2017 +0000 +++ b/SignalNumber.lib Tue Oct 31 14:35:52 2017 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/peterknoben/code/SignalNumber2/#5f8dee4d4b09 +https://os.mbed.com/users/peterknoben/code/SignalNumber2/#15543c143a63
--- a/main.cpp Mon Oct 30 10:43:26 2017 +0000
+++ b/main.cpp Tue Oct 31 14:35:52 2017 +0000
@@ -3,123 +3,157 @@
#include "PIDControl.h"
#include "BiQuad.h"
#include "signalnumber.h"
+#include "Movement.h"
#include "mbed.h"
#include "encoder.h"
#include "Servo.h"
#include "FastPWM.h"
+//This code is for Mbed 2
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
MODSERIAL pc(USBTX, USBRX); //Establish connection
Ticker MyControllerTicker1; //Declare Ticker Motor 1
Ticker MyControllerTicker2; //Declare Ticker Motor 2
-Ticker MySampleTicker; //Declare Ticker HIDscope
+Ticker MySampleTicker; //Declare Ticker HIDscope
Ticker MyTickerMean; //Declare Ticker Signalprocessing
-InterruptIn But2(PTC6); //Declare button for calibration
+InterruptIn But2(PTA4); //Declare button for min calibration
+InterruptIn But1(PTC6); //Declare button for max calibration
AnglePosition Angle; //Declare Angle calculater
PIDControl PID; //Declare PID Controller
-SignalNumber Signal; //Declare Signal determiner
+SignalNumber SignalLeft; //Declare Signal determiner for Left arm
+SignalNumber SignalRight; //Declare Signal determiner for Right arm
+Movement MoveLeft; //Declare Movement determiner
+Movement MoveRight;
-AnalogIn potmeter1(A5);
-AnalogIn potmeter5(A3); //Set Inputpin
-AnalogIn potmeter2(A4); //Set Inputpin
-AnalogIn emg0( A0 ); //Set Inputpin
-AnalogIn emg1( A1 ); //Set Inputpin
-
+AnalogIn emg0( A0 ); //Set Inputpin for EMG 0 signal Left
+AnalogIn emg1( A1 ); //Set Inputpin for EMG 1 signal Left
+AnalogIn emg2( A2 ); //Set Inputpin for EMG 2 signal Right
+AnalogIn emg3( A3 ); //Set Inputpin for EMG 3 signal Right
+AnalogIn emg4( A4 ); //Set Inputpin for EMG 4 signal Mode
+AnalogIn emg5( A5 ); //Set Inputpin for EMG 5 signal Mode
+DigitalOut M( D9 ); //Set digital in for mode selection
+DigitalOut Led_red(LED_RED);
+DigitalOut Led_green(LED_GREEN);
+DigitalOut Led_blue(LED_BLUE);
const float CONTROLLER_TS = 0.02; //Motor frequency
-const float MEAN_TS = 0.1; //Mean value frequency
-const float SAMPLE_TS = 0.02; //BiQuad frequency
+const float MEAN_TS = 0.002; //Filter frequency
+
+//Testing methods
+/*
+AnalogIn potmeter1(A5);
+AnalogIn potmeter5(A3); //Set Inputpin for x axis
+AnalogIn potmeter2(A4); //Set Inputpin for y axis
+*/
+
+//------------------------------------------------------------------------------
+//---------------------------Mode selection-------------------------------------
+//------------------------------------------------------------------------------
+// From the other Mbed there will be send a signal to determine in which mode the system is in
+int mode =0;
+
+//Recieving mode selection from Mbed 1
+void mode_selection(){
+ if(mode ==6){
+ mode=1;
+ }
+ else{
+ mode++;
+ }
+ pc.printf("mode = %i\r\n", mode);
+}
+
//------------------------------------------------------------------------------
//-----------------------------EMG Signals--------------------------------------
//------------------------------------------------------------------------------
-const int n = 10; //Window size for the mean value, also adjust in signalnumber.cpp
+// Filtering the signal and getting the usefull information out of it.
+const int n = 400; //Window size for the mean value, also adjust in signalnumber.cpp
const int action =50; //Number of same mean values to change the signalnumber
-const int m = 10; //Number of samples for calibration
-int mode;
-float emg_offset;
-float meanx;
+const int m = 300; //Number of samples for calibration
+int CaseLeft; //Strength of the muscles Left
+int CaseRight; //Strength of the muscles Right
+float emg_offsetLeft; //Calibtarion value to get zero
+float emg_offsetmaxLeft; //Calibration value to scale to 1
+float emg_offsetRight; //Calibtarion value to get zero
+float emg_offsetmaxRight; //Calibration value to scale to 1
+float meanxL; //Temporary variable for mean value
+float meanxR;
+float kLeft; //Scaling factor mean value
+float kRight; //Scaling factor mean value
-//BiQuad filter
+//BiQuad filter variables
BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923, 0.41279762014290533); //Notch filter Biquad
BiQuadChain BiQuad_filter;
-// Calibration function
-void calibration(){
- emg_offset = Signal.calibrate(m, potmeter5);
- pc.printf("offset = %f \r\n", emg_offset);
+void setled(){
+ Led_red=0;
+ Led_green=1;
+ Led_blue=1;
}
-/*
-//Sample function
-void sample()
-{
- float Signal=((emg0+emg1)/2)-emg_offset;
- float Signal_filtered= BiQuad_filter.step(Signal);
- /* scope
- scope.set(0, emg0.read() );
- scope.set(1, emg1.read() );
- scope.set(2, Signal_filtered);
- scope.send();
- led = !led;
- /
+// Calibration function
+void mincalibration(){
+ pc.printf("start cali \r\n");
+ emg_offsetLeft = SignalLeft.calibrate(m,((emg0+emg1)/2));
+ emg_offsetRight = SignalRight.calibrate(m,((emg2+emg3)/2));
+// pc.printf("calibrated, offset = %f \r\n", emg_offset);
+ Led_green=0;
+ Led_red=0;
}
-*/
+void maxcalibration(){
+ pc.printf("start cali max\r\n");
+ emg_offsetmaxLeft = SignalLeft.calibrate(m,((emg0+emg1)/2))-emg_offsetLeft;
+ emg_offsetmaxRight = SignalRight.calibrate(m,((emg2+emg3)/2))-emg_offsetRight;
+ kLeft = 1/emg_offsetmaxLeft;
+ kRight = 1/emg_offsetmaxRight;
+// pc.printf("calibrated, offset = %f scale = %f \r\n",emg_offsetmax, k);
+ Led_red=1;
+}
+
+//Filter de EMG signals with a BiQuad filter
float Filter(float input0, float input1, float offset){
- float Signal=((input0+input1)/2)-offset;
+ float Signal=input0-offset; //((input0+input1)/2)
float Signal_filtered= BiQuad_filter.step(Signal);
return Signal_filtered;
}
+
+//Determine the signalnumbers (i.e. speed) based on the EMG signals
void signalnumber(){
- float Signal_filtered = Filter(emg0, emg1, emg_offset);
- meanx = Signal.getmean(n, Signal_filtered);
- mode = Signal.getnumber(n, action, Signal_filtered);
- pc.printf("pot = %d after filtering = %f mean = %f Signalnumber = %i \r\n" , potmeter5, meanx, mode);
+ //Left
+ float Signal_filteredLeft = fabs(Filter(emg0, emg1, emg_offsetLeft));
+ meanxL = SignalLeft.getmean(n, Signal_filteredLeft)*kLeft; //Testing variable
+ CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft, kLeft);
+ pc.printf("m %f C %i \r\n",meanxL, CaseLeft); //Testing print
+ //Right
+ float Signal_filteredRight = fabs(Filter(emg2, emg3, emg_offsetRight));
+ meanxR = SignalRight.getmean(n, Signal_filteredRight)*kRight; //Testing variable
+ CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight, kRight);
+ pc.printf("m %f C %i \r\n",meanxR, CaseRight); //Testing print
}
-
//------------------------------------------------------------------------------
//-------------------------Kinematic Constants----------------------------------
//------------------------------------------------------------------------------
-const double RAD_PER_PULSE = 0.00074799825*2;
-const double PI = 3.14159265358979323846;
-const float max_rangex = 500.0;
-const float max_rangey = 300.0;
-const float x_offset = 156.15;
-const float y_offset = -76.97;
-const float alpha_offset = -(21.52/180)*PI;
-const float beta_offset = 0.0;
-const float L1 = 450.0;
-const float L2 = 490.0;
-
-
-//------------------------------------------------------------------------------
-//--------------------------------Servo-----------------------------------------
-//------------------------------------------------------------------------------
-// This will be programmed on a different Mbed
-Servo MyServo(D9); //Declare button
-InterruptIn But1(D8); //Declare used button
-int k=0; //Position flag
-
-void servo_control (){
- if (k==0){
- MyServo = 0;
- k=1;
- }
- else{
- MyServo = 2;
- k=0;
- }
-}
+const double RAD_PER_PULSE = 0.00074799825*2; //Number of radials per pulse from the encoder
+const double PI = 3.14159265358979323846; //Pi
+const float max_rangex = 500.0; //Max range on the x axis
+const float max_rangey = 300.0; //Max range on the y axis
+const float x_offset = 156.15; //Start x position from the shoulder joint
+const float y_offset = -76.97; //Start y position from the shoulder joint
+const float alpha_offset = -(21.52/180)*PI; //Begin angle Alpha at zero zero
+const float beta_offset = 0.0; //Begin angle Beta at zero zero
+const float L1 = 450.0; //Length of the first body
+const float L2 = 490.0; //Length of the second body
//------------------------------------------------------------------------------
@@ -140,13 +174,14 @@
void motor1_control(){
- float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, potmeter1, potmeter2);
+ float *position_math;
+ position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
+ position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
+ float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math[0], position_math[1]);
float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
float error_alpha = reference_alpha-position_alpha;
float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
motor1 = fabs(magnitude1);
-// pc.printf("err_a = %f alpha = %f mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
-// pc.printf("\r\n");
// Determine Motor Direction
if (magnitude1 < 0){
motor1DirectionPin = 1;
@@ -175,13 +210,14 @@
void motor2_control(){
- float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, potmeter1, potmeter2);
+ float *position_math;
+ position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
+ position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
+ float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math[0], position_math[1]);
float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
float error_beta = reference_beta-position_beta;
float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
motor2 = fabs(magnitude2);
-// pc.printf("err2 = %f beta = %f mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
-// pc.printf("\r\n");
//Determine Motor Direction
if (magnitude2 > 0){
motor2DirectionPin = 1;
@@ -196,15 +232,17 @@
int main(){
pc.baud(115200);
+ setled();
BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3);
+ But2.rise(&mincalibration);
+ But1.rise(&maxcalibration);
+// M.rise(&mode_selection);
motor1.period(0.0001f);
motor2.period(0.0001f);
MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);
MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
MyTickerMean.attach(&signalnumber, MEAN_TS);
-// MySampleTicker.attach(&sample, SAMPLE_TS);
- But1.rise(&servo_control);
- But2.rise(&calibration);
+
while(1) {}
}