Diff: main.cpp

Revision:

40:7418f46a1ac0

Parent:

39:d065ad7a978d

Child:

41:9678fd827d25

--- a/main.cpp	Thu Oct 26 13:29:56 2017 +0000
+++ b/main.cpp	Thu Oct 26 14:02:49 2017 +0000
@@ -35,7 +35,7 @@
 BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.8934, 0.9085);
 
 // Controller parameters
-const float k_p = 1;
+const float k_p = 10;
 const float k_i = 0; // Still needs a reasonable value
 const float k_d = 0; // Again, still need to pick a reasonable value
 
@@ -131,18 +131,29 @@
     // Read encoders and EMG signal (unnfiltered reference)
     m1counts = Encoder1.getPulses();
     m2counts = Encoder2.getPulses();
-    float r1_uf = ref1.getReference();
-    float r2_uf = ref2.getReference();
-    pc.printf("In controller ticker \r\n");
+    
+    double m1position = e1.fetchMotorPosition(m1counts);
+    double m2position = e2.fetchMotorPosition(m2counts);
+    
+    // measuring EMG signals to use as basis for reference
+
+    float emg1 = ref1.getReference();
+    float emg2 = ref2.getReference();
+    
+    // Filtering the EMG signals   
     
-    // Filter reference
-    float r1 = HPbqc.step(r1_uf);
-    r1 = fabs(r1);
-    r1 = LPbqc.step(r1);
+    double thet1dot = HPbqc.step(emg1);
+    thet1dot = fabs(thet1dot);
+    thet1dot = LPbqc.step(thet1dot);
+    
 
-    float r2 = HPbqc.step(r2_uf);
-    r2 = fabs(r2);
-    r2 = LPbqc.step(r2);
+    double thet2dot = HPbqc.step(emg2);
+    thet2dot = fabs(thet2dot);
+    thet2dot = LPbqc.step(thet2dot);
+    
+    // Pseudo-integrating prescribed angular velocities to obtain prescribed angles
+    double thet1 = m1position + thet1dot*Ts;
+    double thet2 = m1position + thet2dot*Ts;
     
     // Finite state machine
     switch(currentState){
@@ -164,10 +175,10 @@
                 ledB = 1;
                 // need something here to check if "killswitch" has been pressed (?)
                 // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
-                e1.fetchError(m1counts, r1);
-                e2.fetchError(m2counts, r2);
+                e1.fetchError(m1position, thet1);
+                e2.fetchError(m2position, thet1);
 
-                sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0,0.0); // just send the EMG signal value to HIDscope
+                sendDataToPc(thet1, thet2, e1.e_pos, e2.e_pos, 0.0,0.0); // just send the EMG signal value to HIDscope
                 
                 break;
                 }
@@ -178,8 +189,8 @@
                     }
                 
                 // Compute error
-                e1.fetchError(m1counts, r1);
-                e2.fetchError(m2counts, r2);
+                e1.fetchError(m1position, thet1);
+                e2.fetchError(m2position, thet2);
                 
                 // Compute motor value using controller and set motor
                 float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
@@ -188,7 +199,7 @@
                 motor2.setMotor(motorValue2);
                 
                 // Send data to HIDscope
-                sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, motorValue1, motorValue2);
+                sendDataToPc(thet1, thet2, e1.e_pos, e2.e_pos, motorValue1, motorValue2);
                 
                 // Set LED to blue
                 ledR = 1;
@@ -222,9 +233,7 @@
 
                 // fill the array with sample data if it is not yet filled
                 if(Ncal < calSamples){
-                    pc.printf("%.2f \r\n", r1_uf);
-                    EMGsamples.push_back(r1_uf);
-                    pc.printf("%.2f \r\n", EMGsamples.end());
+                    EMGsamples.push_back(emg1);
                     Ncal++;
                     }
                 // if array is filled compute the mean value and switch to active state
@@ -243,7 +252,7 @@
                 ledR = 1;
                 ledG = 0;
                 ledB = 1;
-                sendDataToPc(r1_uf, r2_uf, 0.0, 0.0, 0.0, 0.0);
+                sendDataToPc(thet1, thet2, 0.0, 0.0, 0.0, 0.0);
                 break;
                 }
         }