Ralph Gerlings
8 option EMG
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 43:a6fbcada47aa
- Parent:
- 42:3aa03b5cefb0
--- a/main.cpp Fri Nov 03 03:52:30 2017 +0000
+++ b/main.cpp Fri Nov 03 08:38:14 2017 +0000
@@ -2,7 +2,7 @@
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
-#include "QEI.h"
+//#include "QEI.h"
//Define Objects
AnalogIn emg1_in( A0 ); // Read Out The Signal
@@ -16,21 +16,21 @@
PwmOut motor1pwm( D5);
PwmOut motor2pwm( D6 );
DigitalOut motor2direction( D7 );
- QEI Encoder1(D10, D11, NC, 64); // Encoder
- QEI Encoder2(D12, D13, NC, 64);
+ //QEI Encoder1(D10, D11, NC, 32); // Encoder
+ //QEI Encoder2(D12, D13, NC, 32);
Ticker main_timer;
Ticker max_read1;
Ticker max_read3;
Ticker Motorcontrol;
- Ticker tencoder;
- HIDScope scope( 4 );
+ HIDScope scope( 6 );
DigitalOut red(LED_RED);
DigitalOut blue(LED_BLUE);
DigitalOut green(LED_GREEN);
- Serial pc(USBTX, USBRX);
+ MODSERIAL pc(USBTX, USBRX);
-// EMG Variables & Constants
+
+// EMG Variables
//Right Biceps
double emg1;
double emg1highfilter;
@@ -63,7 +63,7 @@
double emg4lowfilter;
double max4;
double maxpart4;
- // Moving Average Floats
+// Moving Average Floats
// Right Biceps Movavg
float RB0, RB1, RB2, RB3, RB4, RB5, RB6, RB7, RB8, RB9, RB10, RB11, RB12,
RB13, RB14, RB15, RB16, RB17, RB18, RB19, RB20, RB21, RB22, RB23, RB24,
@@ -102,48 +102,10 @@
RL85, RL86, RL87, MOVAVG_RL;
float AV = 0.02; //multiplication value for adding each movavg value
// This value also adds a very slight gain to every value
-
-// RKI Variables & Constants
- double setpoint1 = 2.35;
- double setpoint2 = 0.785;
- double x = 10.77;
- double y = 15.73;
- float pi = 3.14159265359;
- float a = 22.25; //originally 22.25, makes for xinitial=10.766874 // length of arm 1
- float b = 26.5; //originally 16.48 makes for yinitial=15.733 // length of arm 2
- double alpha_old = 2.35; //INITIAL ANGLES IN RADIANS
- double beta_old = 0.785;
- float delta1;
- float delta2;
- float diffmotor_a;
- float diffmotor_b;
-
-// PID Variables & Constants
- double Kp = 0.5;// you can set these constants however you like depending on trial & error
- double Ki = 0.1;
- double Kd = 0.1;
-
- float last_error1 = 0;
- float new_error1 = 0;
- float change_error1 = 0;
- float total_error1 = 0;
- float pid_term1 = 0;
- float pid_term_scaled1 = 0;
- float last_error2 = 0;
- float new_error2 = 0;
- float change_error2 = 0;
- float total_error2 = 0;
- float pid_term2 = 0;
- float pid_term_scaled2 = 0;
-
-// Motor Variables & Constants
- //float MV1 = 0;
- //float MV2 = 0;
- double count1 = 0;
- double count2 = 0;
- double angle1 = 2.35;
- double angle2 = 0.785;
+// Motor Variables
+ float MV1 = 0;
+ float MV2 = 0;
// BiQuad Filter Settings
// Right Biceps
@@ -164,32 +126,9 @@
BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
//
-// RKI Calculations
-//
-//
+// Finding max values for correct motor switch if the button is pressed
+// calibration of both biceps
-float inversekinematics1(float x, float y){
- // cosine law:
- float c = sqrtf(x*x + y*y); // here comes the relevant math, see my inverse kinematics explaination using cosine law
- float alpha1 = acosf((a*a + c*c - b*b)/(2*a*c));
-
- // remaining parts of alpha:
- float alpha2 = atan2f(y,x);
- float alpha = alpha1 + alpha2;
- return alpha;
-}
-
-float inversekinematics2(float x, float y){
- // cosine law:
- float c = sqrtf(x*x + y*y); // here comes the relevant math, see my inverse kinematics explaination using cosine law
- float beta = acosf((a*a + b*b - c*c)/(2*a*b));
- return beta;
-}
-
-// Finding max values for correct motor switch if the button is pressed
-//
-//
-// calibration of both biceps
void get_max1(){
if (max_reader12==0){
green = !green;
@@ -279,6 +218,7 @@
}
// calibration of both lower arms
+
void get_max3(){
if (max_reader34==0){
green = 1;
@@ -367,10 +307,8 @@
maxpart4 = 0.17*max4; // set cut off voltage at 15% of max for right lower arm
}
-// EMG Filtering & Scope
-//
-//
-void filter(/*double setpoint1, double setpoint2*/) {
+// Filtering & Scope
+void filter() {
// Right Biceps
emg1 = emg1_in.read();
emg1highfilter = filterhigh1.step(emg1);
@@ -508,117 +446,81 @@
// Compare measurement to the calibrated value to decide actions
// more options could be added if the callibration is well defined enough
// This part checks for right biceps contractions only
-/* if (maxpart1<MOVAVG_RB || maxpart2<MOVAVG_LB || maxpart3<MOVAVG_LL || maxpart4<MOVAVG_RL){
- wait(0.15f);*/
- if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
+ if (maxpart1<MOVAVG_RB || maxpart2<MOVAVG_LB || maxpart3<MOVAVG_LL || maxpart4<MOVAVG_RL){
+
+ if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 1;
blue = 1;
green = 0;
- //MV1 = 0.5;
- //MV2 = 0;
- x = x + 0.1;
- if (x > 16) {
- x = 16;
- }
- double alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
- double beta = inversekinematics2(x,y);
- setpoint1 = alpha;
- setpoint2 = beta;
- wait(0.1f);
-
- }
- // This part checks for left biceps contractions only
- else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
+ MV1 = 0.08;
+ MV2 = 0;
+ }
+ // This part checks for left biceps contractions only
+ else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 0;
blue = 1;
green = 1;
- //MV1 = -0.5;
- //MV2 = 0;
- x = x - 0.1;
- if (x < 10.77){
- x = 10.87;
- }
- double alpha = inversekinematics1(x,y);
- double beta = inversekinematics2(x,y);
- setpoint1 = alpha;
- setpoint2 = beta;
- wait(0.1f);
- }
- // This part checks for left lower arm contractions only
- else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
+ MV1 = -0.08;
+ MV2 = 0;
+ }
+ // This part checks for left lower arm contractions only
+ else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 1;
blue = 0;
green = 1;
- //MV1 = 0;
- //MV2 = 0.5;
- y = y + 0.1;
- if (y > 19.5) {
- y = 19.5;
- }
- double alpha = inversekinematics1(x,y);
- double beta = inversekinematics2(x,y);
- setpoint1 = alpha;
- setpoint2 = beta;
- wait(0.1f);
- }
- // This part checks for right lower arm contractions only
- else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
+ MV1 = 0;
+ MV2 = 0.08;
+ }
+ // This part checks for right lower arm contractions only
+ else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
red = 0;
blue = 1;
green = 0;
- //MV1 = 0;
- //MV2 = -0.5;
- y = y - 0.1;
- if (y < 15.73) {
- y = 15.83;
- }
- double alpha = inversekinematics1(x,y);
- double beta = inversekinematics2(x,y);
- setpoint1 = alpha;
- setpoint2 = beta;
- wait(0.1f);
- }
-/* // This part checks for both lower arm contractions only
- else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
+ MV1 = 0;
+ MV2 = -0.08;
+ }
+ // This part checks for both lower arm contractions only
+ else if (maxpart1>MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4<MOVAVG_RL){
+ red = 0;
+ blue = 0;
+ green = 0;
+ MV1 = -0.08;
+ MV2 = -0.08;
+ }
+ // This part checks for both biceps contractions only
+ else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
- //MV1 = -0.5;
- //MV2 = -0.5;
- }
- // This part checks for both biceps contractions only
- else if (maxpart1<MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
- red = 0;
- blue = 0;
- green = 0;
- //MV1 = 0.5;
- //MV2 = 0.5;
- }
- // This part checks for right lower arm & left biceps contractions only
- else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
+ MV1 = 0.08;
+ MV2 = 0.08;
+ }
+ // This part checks for right lower arm & left biceps contractions only
+ else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4<MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
- //MV1 = 0.5;
- //MV2 = -0.5;
- }
- // This part checks for left lower arm & right biceps contractions only
- else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
+ MV1 = 0.08;
+ MV2 = -0.08;
+ }
+ // This part checks for left lower arm & right biceps contractions only
+ else if (maxpart1<MOVAVG_RB && maxpart2>MOVAVG_LB && maxpart3<MOVAVG_LL && maxpart4>MOVAVG_RL){
red = 0;
blue = 0;
green = 0;
- //MV1 = -0.5;
- //MV2 = 0.5;
- }*/
- //}
- else {
- red = 1; // Shut down all led colors if no movement is registered
- blue = 1;
- green = 1;
- //MV1 = 0;
- //MV2 = 0;
- }
-
+ MV1 = -0.08;
+ MV2 = 0.08;
+ }
+ }
+ else {
+ red = 1; // Shut down all led colors if no movement is registered
+ blue = 1;
+ green = 1;
+ MV1 = 0;
+ MV2 = 0;
+ //pc.printf( "No contraction registered\n");
+ }
+
// Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope'
scope.set(0, MOVAVG_RB ); // plot Right biceps voltage
scope.set(1, MOVAVG_LB ); // Plot Left biceps voltage
@@ -628,141 +530,42 @@
}
-void encoders(){
- count1 = Encoder1.getPulses();
- count2 = Encoder2.getPulses();
-}
-
-/*
-
-// PID calculations
-//
-//
-void PIDcalculation1() {
-//PID calculation for motor 1
- filter();
- count1 = Encoder1.getPulses();
- angle1 += (0.000748 * count1);
- new_error1 = setpoint1 - angle1;
-
- change_error1 = new_error1 - last_error1;
- total_error1 += new_error1;
- pid_term1 = (Kp * new_error1) + (Ki * total_error1) + (Kd * change_error1);
- if (pid_term1<-255) {
- pid_term1 = -255;
- }
- if (pid_term1>255) {
- pid_term1 = 255;
- }
- pid_term_scaled1 = abs(pid_term1);
-
- last_error1 = new_error1;
-}
-void PIDcalculation2() {
-//PID calculation for motor 2
- filter();
- count2 = Encoder2.getPulses();
- angle2 += (0.000748 * count2);
- new_error2 = setpoint2 - angle2;
-
- change_error2 = new_error2 - last_error2;
- total_error2 += new_error2;
- pid_term2 = (Kp * new_error2) + (Ki * total_error2) + (Kd * change_error2);
- if (pid_term2<-255) {
- pid_term2 = -255;
- }
- if (pid_term2>255) {
- pid_term2 = 255;
+// check MV1 to see if motor1 needs to be activated
+void SetMotor1(float MV1) {
+ motor1pwm = fabs(MV1); // motor speed
+ if (MV1 >=0) {
+ motor1direction = 1; // clockwise rotation
}
- pid_term_scaled2 = abs(pid_term2);
-
- last_error2 = new_error2;
-}
- */
-// Motor control
-//
-//
-// check to see if motor1 needs to be activated
-void SetMotor1() {
- //PIDcalculation1();
- //filter();
- while (angle1<setpoint1 || angle1>setpoint1 || angle2<setpoint2 || angle2>setpoint2) {
- encoders();
- float count1;
- float count2;
- angle1 += (0.0981 * count1);
- angle2 += (0.0981 * count2);
- if (angle1<setpoint1 && angle2<setpoint2) {
- motor1direction = 1; // counterclockwise rotation
- motor2direction = 1;
- }
- else if (angle1>setpoint1 && angle2<setpoint2) {
- motor1direction = 0; // clockwise rotation
- motor2direction = 1;
- }
- else if (angle1<setpoint1 && angle2>setpoint2) {
- motor1direction = 1;
- motor2direction = 0;
- }
- else if (angle1>setpoint1 && angle2>setpoint2) {
- motor1direction = 0;
- motor2direction = 0;
- }
- if ((angle1<setpoint1 || angle1>setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
- motor1pwm = 0.2;
- motor2pwm = 0.2;
- }
- else if ((angle1<setpoint1 || angle1>setpoint1) && (angle2 == setpoint2)) {
- motor1pwm = 0.2;
- motor2pwm = 0;
- }
- else if ((angle1 == setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
- motor1pwm = 0;
- motor2pwm = 0.2;
- }
- else if ((angle1 == setpoint1) && (angle2 == setpoint2)){
- motor1pwm = 0;
- motor2pwm = 0;
- }
+ else {
+ motor1direction = 0; // counterclockwise rotation
}
}
-/*
-// check if motor1 needs to be activated
-void SetMotor2() {
- filter();
- //PIDcalculation2();
- while (angle2<setpoint2 || angle2>setpoint2) {
- count2 = Encoder2.getPulses();
- angle2 += (0.0981 * count2);
- if (angle2<setpoint2){
- motor1direction = 0; // counterclockwise rotation
- }
- else if (angle2>setpoint2){
- motor1direction = 1; // clockwise rotation
- }
- if (angle2<setpoint2 || angle2>setpoint2) {
- motor2pwm = 0.5;
- }
- else if (angle2 == setpoint2){
- motor2pwm = 0;
- }
+// check MV2 if motor1 needs to be activated
+void SetMotor2(float MV2) {
+ motor2pwm = fabs(MV2); // motor speed
+ if (MV2 >=0) {
+ motor2direction = 1; // clockwise rotation
+ }
+ else {
+ motor2direction = 0; // counterclockwise rotation
}
}
-void MeasureAndControl(void) {
- SetMotor1();
- //SetMotor2();
+void MeasureAndControl(void)
+{
+ // This function measures the potmeter position, extracts a
+ // reference velocity from it, and controls the motor with
+ // a simple FeedForward controller. Call this from a Ticker.
+ SetMotor1(MV1);
+ SetMotor2(MV2);
}
-*/
+
int main(){
- pc.baud(115200);
+
main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
max_read3.attach(&get_max3, 2);
- tencoder.attach(&encoders, 0.001);
- Motorcontrol.attach(&SetMotor1,0.1);
- //PIDtimer.attach(&PIDcalculation1, 0.005);
- //PIDtimer.attach(&PIDcalculation2, 0.005);
+ Motorcontrol.attach(&MeasureAndControl,0.5);
while(1) {}
}
\ No newline at end of file