Diff: main.cpp

Revision:

6:9c8e91bb1316

Parent:

5:67afe491a1e3

Child:

7:4df9f6ebe744

--- a/main.cpp	Sat Oct 10 20:02:16 2015 +0000
+++ b/main.cpp	Sat Oct 10 20:38:38 2015 +0000
@@ -37,12 +37,20 @@
     AnalogIn potright(A0);      // define the potmeter outputpins
     AnalogIn potleft(A1);
 
-// BUTTONS              
+// BUTTONS 
+    // control flow             
     DigitalIn   buttonlinks(PTA4);       
     DigitalIn   buttonrechts(PTC6);
         // init values
         bool loop_start = false;
         bool calib_start = false;
+        
+    // direction control
+    DigitalIn   reverse_button_links(D0);
+    DigitalIn   reverse_button_rechts(D1); 
+        // init values
+        bool reverse_links = false;
+        bool reverse_rechts = false;
 
 // LED
     DigitalOut ledred(LED1);
@@ -53,11 +61,7 @@
     // Define signal amplification  (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!)    ??
     double signalamp1 = 1;
     double signalamp2 = 1;
-// Define gain and offset of the input(input between 0 and 1 is optimized). For EMG use 0, 1 and false, for POT use 0.5, 2 and true
-    double offset = 0.5;
-    double gain = 2; 
-    bool POT_in = true;     // show potmeter is attached, increases the range for which 0 is the output.
-// Define storage variables for reference values
+ // Define storage variables for reference values
     double c_reference1 = 0;
     double c_reference2 = 0;
 // Define the maximum rate of change for the reference (velocity)
@@ -88,8 +92,8 @@
         double m_f_v2 = 0;
 
 // Filter coefficients
-        // differential action filter
-        const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0;      // coefficients from sheets are used as first test.
+// differential action filter
+    const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0;      // coefficients from sheets are used as first test.
     
 // tweede orde notch filter 50 Hz
 // biquad 1 coefficienten
@@ -150,12 +154,7 @@
 // to create a reference that moves with a variable speed. It is now set up for potmeter values.  
 double reference_f(double input, double &c_reference)
 {
-    double  potset = (input-offset)*gain;
-    if(POT_in == true && potset < 0.15 && potset > -0.15) // larger area for potmeter to get a zero value
-    { 
-        potset = 0;
-    }
-    double reference = c_reference + potset * controlstep * Vmax ;
+    double reference = c_reference + input * controlstep * Vmax ;
            c_reference = reference; // change the global variable to the latest location.
     return reference;
 }
@@ -252,12 +251,12 @@
 {
     
     double input1 = potright.read()*signalamp1;
+    if(input1 < 0.25) {input1 = 0;}
+    if(input1 > 1) {input1 = 1;}
+    if(reverse_rechts == true) {input1 = -input1;}
     double reference1 = reference_f(input1, c_reference1);      // determine the reference that has been set by the inputsignal
     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
     double error1 = (reference1 - rads1);                       // determine the error (reference - position)
-    scope.set(0,reference1);
-    scope.set(1,rads1);
-    scope.send();   
     double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
     pc.printf("output 1 %f \n",output1);
     if(output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
@@ -273,13 +272,12 @@
 void motor2_control()
 {
     double input2 = potleft.read()*signalamp2;      // replace potleft with filter
-    double reference2 = reference_f(input2, c_reference2);      // determine the reference that has been set by the inputsignal
+    if(input2 < 0.25) {input2 = 0;}
+    if(input2 > 1) {input2 = 1;}
+    if(reverse_links == true) {input2 = -input2;}
+    double reference2 = reference_f(input2, c_reference2);      // determine the reference that has been set by the clamped inputsignal
     double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
     double error2 = (reference2 - rads2);                       // determine the error (reference - position)
-    // scope.set(3,reference2);                                         
-    // scope.set(4,rads2);
-    // scope.send();
-    
     double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
     if(output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
         motor2_rich.write(0);
@@ -312,21 +310,33 @@
     // controller2.attach(&motor2_activate, controlstep);   Disabled while debugging PID-controller ??
     while(true) 
     {
-        // button functions
+        // button press functions
+        // flow buttons
         if(buttonlinks.read() == 0)
         {
-            loop_start = !loop_start;     // reverse the boolean loop_start value
+            loop_start = !loop_start;     
             wait(buttonlinks.read() == 1);
-            wait(0.3);             // check if it causes instability!! ??
+            wait(0.3);             
         }
-        
         if(buttonrechts.read() == 0)
         {
-            calib_start = !calib_start;     // reverse the boolean calib_start value
+            calib_start = !calib_start;     
             wait(buttonrechts.read() == 1);
-            wait(0.3);             // check if it causes instability!! ??
+            wait(0.3);             
         }
-        
+        // reverse buttons
+        if(reverse_button_links.read() == 0)
+        {
+            reverse_links = !reverse_links;     
+            wait(reverse_button_links.read() == 1);
+            wait(0.3);             
+        }
+        if(reverse_button_rechts.read() == 0)
+        {
+            reverse_rechts = !reverse_rechts;     
+            wait(reverse_button_rechts.read() == 1);
+            wait(0.3);             
+        }
         // Main Control stuff and options
         if(loop_start == true && calib_start == false)        // check if start button = true then start the main control loops
         {