Diff: main.cpp

Revision:

43:dd0888f86357

Parent:

42:ae78ff03d9d6

Child:

44:d157094b48d5

diff -r ae78ff03d9d6 -r dd0888f86357 main.cpp
--- a/main.cpp	Fri Oct 27 10:37:43 2017 +0000
+++ b/main.cpp	Wed Nov 01 14:54:50 2017 +0000
@@ -12,32 +12,33 @@
 #include "motorConfig.h"
 #include "errorFetch.h"
 #include "BiQuad.h"
+#include "inverseKinematics.h"
 
 // ADJUSTABLE PARAMETERS
+// robot dimensions
+const float L1 = 0.391;
+const float L2 = 0.391;
+
 // controller ticker time interval
-const float Ts = 0.01;
+const float Ts = 0.008;
+
+// Defining an inverse-kinematics calculator
+inverseKinematics robotKinematics(L1,L2,Ts);
 
 // EMG filter parameters
 // calibration time
 const int calSamples = 1000;
 
+// KINEMATICS reference motor position
+volatile double Mp1C = 0;
+volatile double Mp2C = 0;
+
 // Initialize average and max EMG value for calibration to 0 and 1 respectively
 volatile float avgEMGvalue = 0;
 volatile double maxEMGvalue = 1;
 
-// high pass
-BiQuadChain HPbqc;
-BiQuad HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
-BiQuad HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904);
-
-
-// low pass
-BiQuadChain LPbqc;
-BiQuad LPbq1(1.0e-7*1.2023, 1.0e-7*2.4046, 1.0e-7*1.2023, 1.0000, -1.9313, 0.9327);
-BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.9702, 0.9716);
-
 // Controller parameters
-const float k_p = 1;
+const float k_p = 0.01;
 const float k_i = 0; // Still needs a reasonable value
 const float k_d = 0; // Again, still need to pick a reasonable value
 
@@ -46,7 +47,7 @@
 
 // LOGISTICS
 // Declaring finite-state-machine states
-enum robotStates {KILLED, ACTIVE, CALIBRATING};
+enum robotStates {KILLED, ACTIVE};
 volatile robotStates currentState = KILLED;
 volatile bool stateChanged = true;
 
@@ -72,9 +73,6 @@
 InterruptIn sw3(SW3);
 InterruptIn button1(D2);
 InterruptIn button2(D3);
-//AnalogIn pot2(A2);
-//AnalogIn emg0( A0 );
-//AnalogIn emg1( A1 );
 
 // Defining LED outputs to indicate robot state-us
 DigitalOut ledG(LED_GREEN);
@@ -90,14 +88,12 @@
 volatile float w;
 
 
-void sendDataToPc(float data1, float data2, float data3, float data4, float data5, float data6){
+void sendDataToPc(float data1, float data2, float data3, float data4){
         // Capture data
         x = data1;
         y = data2;
         z = data3;
         q = data4;
-        k = data5;
-        w = data6;
         scope.set(0, x);   
         scope.set(1, y); 
         scope.set(2, z);
@@ -113,9 +109,11 @@
 const float maxAngle = 1*3.14*posRevRange; // max angle in radians
 
 
-// Function getReferencePosition returns reference angle based on potmeter 1
-refGen ref1(A2, maxAngle);
-refGen ref2(A3, maxAngle);
+// References based on potmeter 1 and 2
+// Set Vx using pot1 = A5
+// Set Vy using pot2 = A4
+refGen ref1(A4, 0.1);
+refGen ref2(A3, 0.1);
 
 // readEncoder reads counts and revs and logs results to serial window
 errorFetch e1(gearRatio, Ts);
@@ -130,39 +128,15 @@
 
 // PROBLEM: if I'm processing the state machine in the endless while loop, how can I adjust robot behavior in the ticker (as it'll keep running)? Do I need to also implement it there? If so, why bother with the while(1) in the main function in the first place?
 void measureAndControl(){
-    // Read encoders and EMG signal (unnfiltered reference)
+    // Read encoders and potmeter signal (unnfiltered reference)
     m1counts = Encoder1.getPulses();
     m2counts = Encoder2.getPulses();
     
     double m1position = e1.fetchMotorPosition(m1counts);
     double m2position = e2.fetchMotorPosition(m2counts);
     
-    // measuring and normalizing EMG signals to use as basis for reference
-
-    double emg1 = ref1.getReference();
-    double emg2 = ref2.getReference();
-    
-    
-    // Filtering the EMG signals   
-
-    double emg1HP = HPbqc.step(emg1);
-    double emg1HP_abs = fabs(emg1HP);
-    double emg1HPLP_abs = LPbqc.step(emg1HP_abs);
-//    thet1 = fabs(thet1);
-//    thet1 = LPbqc.step(thet1);
-    
-
-    double emg2HP = HPbqc.step(emg2);
-    double emg2HP_abs = fabs(emg2);
-    double emg2HPLP_abs = LPbqc.step(emg2HP_abs);
-//    thet2 = LPbqc.step(thet2);
-
-    // Something worth trying: set a position setpoint that constantly changes but will never be reached until EMG value is 0 as it is computed from the robot's current position    
-//    thet1 = m1position + thet1;
-//    thet2 = m1position + thet2;
-    
-    // Other possibility: use thet1 and thet2 directly as reference angles. That'll require the user to hold muscle tension to stay in a certain position though
-    
+    double pot1 = ref1.getReference();
+    double pot2 = ref2.getReference();
     
     // Finite state machine
     switch(currentState){
@@ -175,31 +149,43 @@
                     pc.printf("Killed state \r\n");
                     stateChanged = false;               
                     }
-
-                // Send reference data to pc
                 
                 // Set LED to red
                 ledR = 0;
                 ledG = 1;
                 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(m1position, thet1);
-//                e2.fetchError(m2position, thet1);
 
-                sendDataToPc(emg1, emg1HP_abs, emg1HPLP_abs, emg2, emg2HP, emg2HPLP_abs); // just send the EMG signal value to HIDscope
-                
+                sendDataToPc(pot1, pot2, m1counts, m2counts); // just send the EMG signal value to HIDscope
                 break;
                 }
+                
+                
             case ACTIVE:
                 {
                 if(stateChanged){
                     pc.printf("Active state \r\n");
+                    Mp1C = m1position;
+                    Mp2C = m2position;
+                    stateChanged = false;
                     }
+                    
+                // Using potmeter signals to define a desired end-effector velocity;
+                
+                double vx = pot1;
+                double vy = pot2;
                 
-                // Compute error
-                e1.fetchError(m1position, emg1);
-                e2.fetchError(m2position, emg2);
+                // Translating vx and vy to angular velocities
+                Matrix q_dot = robotKinematics.computeAngularVelocities(vx,vy,Mp1C,Mp2C);
+                double q_dot1 = q_dot(1,1);
+                double q_dot2 = q_dot(2,1);
+                
+                // Computing position setpoint for next ticker tick using desired end-effector velocity
+                double Mp1N = Mp1C + Ts*q_dot1;
+                double Mp2N = Mp2C + Ts*q_dot2;
+                
+                // Compute error between actual CURRENT motor position and NEXT position setpoint
+                e1.fetchError(m1position, Mp1N);
+                e2.fetchError(m2position, Mp2N);
                 
                 // Compute motor value using controller and set motor
                 float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
@@ -208,7 +194,11 @@
                 motor2.setMotor(motorValue2);
                 
                 // Send data to HIDscope
-                sendDataToPc(emg1, emg2, e1.e_pos, e2.e_pos, motorValue1, motorValue2);
+                sendDataToPc(Mp1N, Mp2N, q_dot1, q_dot2);
+                
+                // Prepare for next round
+                Mp1C = Mp1N;
+                Mp2C = Mp2N;
                 
                 // Set LED to blue
                 ledR = 1;
@@ -217,59 +207,7 @@
                 // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
                 break;
                 }
-            case CALIBRATING:
-                {
-                // NOTE: maybe wrap this whole calibrating thing in a library?
-                
-               // Do I need to kill motors here?
-
-                
-                // Initialization of CALIBRATE state
-                static int Ncal = 0;
-                std::vector<float> EMGsamples;
-                
-                if(stateChanged){                                        
-                    // Kill motors
-                    pc.printf("Calibrate state \r\n");
-                    motor1.kill();
-                    motor2.kill();
-                    
-                    // Clear sample value vector and reset counter variable
-                    EMGsamples.clear();
-                    Ncal = 0;
-                    stateChanged = false;
-                    }
-
-                // fill the array with sample data if it is not yet filled
-                if(Ncal < calSamples){
-                    EMGsamples.push_back(emg1);
-                    Ncal++;
-                    }
-                // if array is filled compute the mean value and switch to active state
-                else {
-                    // Set new avgEMGvalue
-                    avgEMGvalue = std::accumulate(EMGsamples.begin(), EMGsamples.end(), 0)/calSamples; // still needs to return this value 
-                    double maxEMGvalue = *std::max_element(EMGsamples.begin(), EMGsamples.end());
-                    
-                    
-                    pc.printf("Avg emg value is %.2f changed state to active", avgEMGvalue);
-                    // State transition logic
-                    currentState = ACTIVE;
-                    stateChanged = true;
-                    Ncal = 0;
-                    }
-                
-                // Set LED to green
-                ledR = 1;
-                ledG = 0;
-                ledB = 1;
-//                sendDataToPc(thet1, thet2, 0.0, 0.0, 0.0, 0.0);
-                break;
-                }
         }
-    
-    
-    
     }
 
 void r1SwitchDirection(){
@@ -292,17 +230,11 @@
     stateChanged = true;
     }
     
-void calibrateRobot(){
-    currentState = CALIBRATING;
-    stateChanged = true;
-    }
 
 int main()
     {
     pc.baud(115200);
     pc.printf("Main function");
-    HPbqc.add(&HPbq1).add(&HPbq2);
-    LPbqc.add(&LPbq1).add(&LPbq2);
     
     // Attaching state change functions to buttons;
     sw2.fall(&killSwitch);