Diff: main.cpp

Revision:

38:f1d2d42a4bdc

Parent:

37:633dd1901681

Child:

39:d065ad7a978d

--- a/main.cpp	Mon Oct 23 13:50:49 2017 +0000
+++ b/main.cpp	Tue Oct 24 13:17:42 2017 +0000
@@ -14,7 +14,7 @@
 
 // ADJUSTABLE PARAMETERS
 // controller ticker time interval
-const float Ts = 0.1;
+const float Ts = 0.01;
 
 // EMG filter parameters
 // calibration time
@@ -68,7 +68,7 @@
 InterruptIn sw3(SW3);
 InterruptIn button1(D2);
 InterruptIn button2(D3);
-//AnalogIn pot2(A3);
+//AnalogIn pot2(A2);
 //AnalogIn emg0( A0 );
 //AnalogIn emg1( A1 );
 
@@ -101,22 +101,22 @@
 
 
 // REFERENCE PARAMETERS
-int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
+int posRevRange = 5; // describes the ends of the position range in complete motor output shaft revolutions in both directions
 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
 
 
 // Function getReferencePosition returns reference angle based on potmeter 1
 refGen ref1(A1, maxAngle);
-//refGen ref2(A2, maxAngle);
+refGen ref2(A1, maxAngle);
 
 // readEncoder reads counts and revs and logs results to serial window
 errorFetch e1(gearRatio, Ts);
-//errorFetch e2(gearRatio, Ts);
+errorFetch e2(gearRatio, Ts);
 
 // Generate a PID controller with the specified values of k_p, k_d and k_i
 controller motorController1(k_p, k_d, k_i);
-    
-// setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
+controller motorController2(k_p, k_d, k_i);
+
 motorConfig motor1(D4,D5);
 motorConfig motor2(D7,D6);
 
@@ -126,8 +126,12 @@
     m1counts = Encoder1.getPulses();
     m2counts = Encoder2.getPulses();
     float r1_uf = ref1.getReference();
-    //float r2_uf = ref2.getReference();
+    float r2_uf = ref2.getReference();
     pc.printf("In controller ticker \r\n");
+    
+    float r1 = r1_uf;
+    float r2 = r2_uf;
+    
     // Finite state machine
     switch(currentState){
             case KILLED:
@@ -135,13 +139,12 @@
                 // Initialization of KILLED state: cut power to both motors
                 if(stateChanged){
                     motor1.kill();
-                    //motor2.kill();
+                    motor2.kill();
                     pc.printf("Killed state \r\n");
                     stateChanged = false;               
                     }
 
                 // Send reference data to pc
-                sendDataToPc(r1_uf, r1_uf, r1_uf, r1_uf, r1_uf); // just send the EMG signal value to HIDscope
                 
                 // Set LED to red
                 ledR = 0;
@@ -149,6 +152,11 @@
                 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);
+
+                sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0); // just send the EMG signal value to HIDscope
+                
                 break;
                 }
             case ACTIVE:
@@ -161,21 +169,24 @@
                 r1 = fabs(r1);
                 r1 = LPbqc.applyFilter(r1);
 
-                //float r2 = HPbqc.applyFilter(r2_uf);
-                //r2 = fabs(r2);
-                //r2 = LPbqc.applyFilter(r2);
+                float r2 = HPbqc.applyFilter(r2_uf);
+                r2 = fabs(r2);
+                r2 = LPbqc.applyFilter(r2);
 
                 
+                
                 // Compute error
                 e1.fetchError(m1counts, r1);
-                //e2.fetchError(m2counts, r2);
+                e2.fetchError(m2counts, r2);
                 
                 // Compute motor value using controller and set motor
-                float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
-                motor1.setMotor(motorValue);
+                float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
+                float motorValue2 = motorController2.control(e2.e_pos, e2.e_int, e2.e_der);
+                motor1.setMotor(motorValue1);
+                motor2.setMotor(motorValue2);
                 
                 // Send data to HIDscope
-                sendDataToPc(r1_uf, r1, e1.e_pos, e1.e_der, motorValue);
+                sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0);
                 
                 // Set LED to blue
                 ledR = 1;
@@ -199,7 +210,7 @@
                     // Kill motors
                     pc.printf("Calibrate state \r\n");
                     motor1.kill();
-                    //motor2.kill();
+                    motor2.kill();
                     
                     // Clear sample value vector and reset counter variable
                     EMGsamples.clear();
@@ -230,7 +241,7 @@
                 ledR = 1;
                 ledG = 0;
                 ledB = 1;
-                sendDataToPc(r1_uf, r1_uf, r1_uf, r1_uf, r1_uf);
+                sendDataToPc(r1_uf, r2_uf, 0.0, 0.0, 0.0);
                 break;
                 }
         }
@@ -239,10 +250,15 @@
     
     }
 
-void rSwitchDirection(){
+void r1SwitchDirection(){
     ref1.r_direction = !ref1.r_direction;
     pc.printf("Switched reference direction! \r\n");
     }
+    
+void r2SwitchDirection(){
+    ref2.r_direction = !ref2.r_direction;
+    pc.printf("Switched reference direction! \r\n");
+    }
 
 void killSwitch(){
     currentState = KILLED;
@@ -266,8 +282,8 @@
     // Attaching state change functions to buttons;
     sw2.fall(&killSwitch);
     sw3.fall(&activateRobot);
-    button1.rise(&calibrateRobot);
-    button2.rise(&rSwitchDirection);
+    button1.rise(&r1SwitchDirection);
+    button2.rise(&calibrateRobot);
     
     controllerTicker.attach(measureAndControl, Ts);
     pc.printf("Encoder ticker attached and baudrate set");