Ralph Gerlings
8 option EMG
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 33:97e69c32a768
- Parent:
- 32:a779b1131977
- Child:
- 34:6e74f6629a0e
--- a/main.cpp Thu Nov 02 13:10:28 2017 +0000
+++ b/main.cpp Fri Nov 03 00:25:57 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,20 @@
PwmOut motor1pwm( D5);
PwmOut motor2pwm( D6 );
DigitalOut motor2direction( D7 );
- //QEI Encoder1(D10, D11, NC, 32); // Encoder
- //QEI Encoder2(D12, D13, NC, 32);
+ QEI Encoder1(D10, D11, NC, 64); // Encoder
+ QEI Encoder2(D12, D13, NC, 64);
Ticker main_timer;
Ticker max_read1;
Ticker max_read3;
Ticker Motorcontrol;
- HIDScope scope( 6 );
+ HIDScope scope( 4 );
DigitalOut red(LED_RED);
DigitalOut blue(LED_BLUE);
DigitalOut green(LED_GREEN);
- MODSERIAL pc(USBTX, USBRX);
+ Serial pc(USBTX, USBRX);
-
-// EMG Variables
+// EMG Variables & Constants
//Right Biceps
double emg1;
double emg1highfilter;
@@ -63,7 +62,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,10 +101,48 @@
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
+ float setpoint1;
+ float setpoint2;
+ 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
+ float alpha_old = 2.35; //INITIAL ANGLES IN RADIANS
+ float beta_old = 0.785;
+ float delta1;
+ float delta2;
+ float x = 10.77;
+ float y = 15.73;
+ 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;
-// Motor Variables
- float MV1 = 0;
- float MV2 = 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;
// BiQuad Filter Settings
// Right Biceps
@@ -126,9 +163,32 @@
BiQuad filterlow4(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
//
+// RKI Calculations
+//
+//
+
+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;
@@ -213,12 +273,11 @@
wait(0.2f);
green = 1;
}
- maxpart1 = 0.14*max1; // set cut off voltage at 13% of max for right biceps
+ maxpart1 = 0.15*max1; // set cut off voltage at 13% of max for right biceps
maxpart2 = 0.15*max2; // set cut off voltage at 13% of max for left biceps
}
// calibration of both lower arms
-
void get_max3(){
if (max_reader34==0){
green = 1;
@@ -307,7 +366,9 @@
maxpart4 = 0.17*max4; // set cut off voltage at 15% of max for right lower arm
}
-// Filtering & Scope
+// EMG Filtering & Scope
+//
+//
void filter() {
// Right Biceps
emg1 = emg1_in.read();
@@ -446,78 +507,100 @@
// 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){
+/* 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){
red = 1;
blue = 1;
green = 0;
- MV1 = 0.5;
- MV2 = 0;
- }
- // This part checks for left biceps contractions only
- else if (maxpart1>MOVAVG_RB && maxpart2<MOVAVG_LB && maxpart3>MOVAVG_LL && maxpart4>MOVAVG_RL){
+ //MV1 = 0.5;
+ //MV2 = 0;
+ x = x + 0.5;
+ float alpha = inversekinematics1(x,y); //calculate alpha and beta for current x,y
+ float beta = inversekinematics2(x,y);
+ setpoint1 = alpha;
+ setpoint2 = beta;
+ }
+ // 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;
- }
- // 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.5;
+ //MV2 = 0;
+ x = x - 0.5;
+ float alpha = inversekinematics1(x,y);
+ float beta = inversekinematics2(x,y);
+ setpoint1 = alpha;
+ setpoint2 = beta;
+ }
+ // 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;
- }
- // 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.5;
+ y = y + 0.5;
+ float alpha = inversekinematics1(x,y);
+ float beta = inversekinematics2(x,y);
+ setpoint1 = alpha;
+ setpoint2 = beta;
+ }
+ // 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;
- }
- // 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.5;
+ y = y - 0.5;
+ float alpha = inversekinematics1(x,y);
+ float beta = inversekinematics2(x,y);
+ setpoint1 = alpha;
+ setpoint2 = beta;
+ }
+/* // 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.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){
+ //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.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){
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.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){
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;
- //pc.printf( "No contraction registered\n");
- }
-
+ //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;
+ }
+
// 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
@@ -527,39 +610,95 @@
}
-// 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 calculations
+//
+//
+void PIDcalculation() {
+//PID calculation for motor 1
+ count1 = Encoder1.getPulses();
+ angle1 += (0.0981 * count1);
+ new_error1 = setpoint1 - alpha_old;
+
+ 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;
+
+//PID calculation for motor 2
+ count2 = Encoder2.getPulses();
+ angle2 += (0.0981 * count2);
+ new_error2 = setpoint2 - beta_old;
+
+ 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;
+ }
+ pid_term_scaled2 = abs(pid_term2);
+
+ last_error2 = new_error2;
+}
+
+// Motor control
+//
+//
+// check to see if motor1 needs to be activated
+void SetMotor1(float angle1, float setpoint1) {
+ PIDcalculation();
+ if (angle1<setpoint1){
+ motor1direction = 0; // counterclockwise rotation
}
else {
- motor1direction = 0; // counterclockwise rotation
+ 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) {
+// check if motor1 needs to be activated
+void SetMotor2(float angle2, float setpoint2) {
+ PIDcalculation();
+ if (angle2<setpoint2){
+ motor2direction = 0; // counterclockwise rotation
+ }
+ else {
motor2direction = 1; // clockwise rotation
}
- else {
- motor2direction = 0; // counterclockwise rotation
+ if (angle2<setpoint2 || angle2>setpoint2) {
+ motor2pwm = 0.5;
+ }
+ else if (angle2 == setpoint2){
+ motor2pwm = 0;
}
}
-void MeasureAndControl(void)
-{
+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);
+ SetMotor1(angle1,setpoint1);
+ SetMotor2(angle2,setpoint2);
}
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);