Diff: main.cpp

Revision:

22:31065a83d9e8

Parent:

20:31876566d70f

Parent:

17:1f93c83e211f

Child:

23:7d83af123c43

--- a/main.cpp	Fri Oct 26 12:59:52 2018 +0000
+++ b/main.cpp	Mon Oct 29 12:21:26 2018 +0000
@@ -5,6 +5,7 @@
 #include "BiQuad.h"
 #include "PID_controller.h"
 #include "kinematics.h"
+#include "processing_chain_emg.h"
 
 //Define objects
 MODSERIAL pc(USBTX, USBRX);
@@ -20,30 +21,44 @@
 PwmOut      motor2_pwm(D7);
 DigitalOut  motor2_dir(D6);
 
+// defining encoders
+QEI motor_1_encoder(D12,D13,NC,32);
+QEI motor_2_encoder(D10,D11,NC,32);
+
+// other objects
 AnalogIn    potmeter1(A2);
 AnalogIn    potmeter2(A3);
 DigitalIn   button(D0);
 
-Ticker Sample;
+// tickers and timers
+Ticker loop_ticker;
 Timer state_timer;
 
 enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //All possible robot states
 
 //Global variables/objects
 States current_state;
+<<<<<<< local
 Ticker loop_ticker; //The Ticker object that will ensure perfect timing of our looping code
 float e, u1, u2, emg_signal_raw_0, processed_emg_0, emg_signal_raw_1, processed_emg_1, robot_end_x, robot_end_y, reference_end_x, reference_end_y, motor_angle_1, motor_angle_2, motor_counts_1, motor_counts_2, q_ref_1, q_ref_2; //will be set by the motor_controller function
+=======
+
+double des_vx, des_vy, x, y, q1, q2, qref1, qref2, e1, e2, u1, u2, raw_emg_0, process_emg_0, raw_emg_1, process_emg_1; //will be set by the motor_controller function
+double vxmax = 1.0, vymax = 1.0;
+>>>>>>> other
 int counts_per_rotation = 32;
 bool state_changed = false;
+<<<<<<< local
 double x; // Making the position (x,y) of the end effector global
 double y;
+=======
+const double T = 0.001;
+>>>>>>> other
 
-float processing_chain_emg(int num) {
-    return 6.0;
-}
-
+// Functions
 void measure_all() 
 {
+<<<<<<< local
     motor_angle_1 = motor_counts_1*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
     motor_angle_2 = motor_counts_2*2.0f*3.1415926535f/counts_per_rotation;
     forwardkinematics_function(motor_angle_1,motor_angle_2);  //motor_angle is global, this function ne
@@ -52,6 +67,14 @@
     emg_signal_raw_1 = emg1.read();
     processed_emg_0 = processing_chain_emg(0);  // some function ‘float my_emg_processing_chain()’ that returns a float. The raw emg is global
     processed_emg_1 = processing_chain_emg(1);
+=======
+    q1 = motor_1_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
+    q2 = motor_2_encoder.getPulses()*2.0f*3.1415926535f/counts_per_rotation;
+    forwardkinematics_function(q1,q2,x,y);  //motor_angle is global, this function ne
+    raw_emg_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
+    raw_emg_1 = emg1.read();
+    processing_chain_emg(raw_emg_0, raw_emg_1, process_emg_0, process_emg_1);  // processes the emg signals
+>>>>>>> other
 }
 
 void output_all() {
@@ -70,6 +93,7 @@
             if (button.read()==true) 
             {
                 current_state = calib_enc; //the NEXT loop we will be in calib_enc state
+                // no state_changed action?
             }
             break; //to avoid falling through to the next state, although this can sometimes be very useful.
         
@@ -89,7 +113,7 @@
             break;
             
         case calib_emg:     //calibrate emg-signals
-            
+            current_state = operational;
             break;
         
         case operational:       //interpreting emg-signals to move the end effector
@@ -98,18 +122,33 @@
             reference_end_x = robot_end_x + processed_emg_0;
             reference_end_y = robot_end_y + processed_emg_0;
             
+<<<<<<< local
+=======
+            // here we have to determine the desired velocity based on the processed emg signals and calibration
+            if (process_emg_0 >= 0.16) { des_vx = vxmax; }
+            else if(process_emg_0 >= 0.09) { des_vx = vxmax * 0.66; }
+            else if(process_emg_0 >= 0.02) { des_vx = vxmax * 0.33; }
+            else { des_vx = 0; }
+            
+            if (process_emg_1 >= 0.16) { des_vy = vymax; }
+            else if(process_emg_1 >= 0.09) { des_vy = vymax * 0.66; }
+            else if(process_emg_1 >= 0.02) { des_vy = vymax * 0.33; }
+            else { des_vy = 0; }
+            
+>>>>>>> other
             if (button.read() == true) { current_state = demo; }
             
             break;
             
         case demo: //moving according to a specified trajectory
             
-            if (button.read() == true) { current_state = demo; }
+            if (button.read() == true) { current_state = operational; }
             
             break;
         
         case failure: //no way to get out
             u1 = 0.0f;
+            u2 = 0.0f;
             break;
     }
 }
@@ -117,10 +156,10 @@
 void motor_controller() 
 {
     if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
-        q_ref += inversekinematics_function(reference_end_point)/samplingfreq; //many different states can modify your reference position, so just do the inverse kinematics once, here
-        e1 = q_ref - motor_angle; //tracking error (q_ref - q_meas)
-        e2 = q_ref - motor_angle;
-        PID_controller(e1,e2,u1,u2); //feedback controller or with possibly fancy controller additions...; pass by reference
+        inversekinematics_function(x,y,T,qref1,qref2,q1,q2,des_vx,des_vy); //many different states can modify your reference position, so just do the inverse kinematics once, here
+        e1 = qref1 - q1; //tracking error (q_ref - q_meas)
+        e2 = qref2 - q2;
+        PID_controller(e1,e2,u1,u2,T); //feedback controller or with possibly fancy controller additions...; pass by reference
         } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
 }
 
@@ -141,7 +180,8 @@
     current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
     u1 = 0.0f; //initial output to motors is 0.
     u2 = 0.0f;
-    loop_ticker.attach(&loop_function, 1/samplingfreq); //Run the function loop_function 1000 times per second
+    bqc.add(&bqhigh).add(&bqnotch);        // filter cascade for emg
+    loop_ticker.attach(&loop_function, T); //Run the function loop_function 1000 times per second
     
     while (true) { }  //Do nothing here (timing purposes)
 }