Diff: main.cpp

Revision:

2:2563df4ee829

Parent:

1:2f23368e8638

Child:

3:7273bbe6aa02

--- a/main.cpp	Wed Oct 07 18:19:07 2015 +0000
+++ b/main.cpp	Fri Oct 09 19:34:14 2015 +0000
@@ -1,43 +1,58 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "encoder.h"
-// #include "HIDScope.h"
+#include "HIDScope.h"
 
 Serial pc(USBTX,USBRX);
-// HIDScope scope(4);          // definieerd het aantal kanalen van de scope
+HIDScope scope(2);          // definieerd het aantal kanalen van de scope
 
-// Define Tickers and stuff
+// Define Tickers and control frequencies
 Ticker          controller1, controller2;        // definieer de ticker die controler1 doet
-double controlfreq = 10 ;    // controlloops frequentie (Hz)
-double controlstep = 1/controlfreq; // timestep derived from controlfreq
+    // Go flag variables
+    volatile bool motor1_go = false, motor2_go = false;
+
+    // Frequency control
+    double controlfreq = 50 ;    // controlloops frequentie (Hz)
+    double controlstep = 1/controlfreq; // timestep derived from controlfreq
 
-//MOTOR INPUTPINS
+
+//MOTOR OUTPUTPINS
 // motor 1
-PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
-DigitalOut motor1_rich(D7); // digitaal signaal voor richting
+    PwmOut motor1_aan(D6);      // PWM signaal motor 2 (uit sheets)
+    DigitalOut motor1_rich(D7); // digitaal signaal voor richting
 // motor 2
-PwmOut motor2_aan(D5);
-DigitalOut motor2_rich(D4);
+    PwmOut motor2_aan(D5);
+    DigitalOut motor2_rich(D4);
 
-// ENCODER
+// ENCODER INPUTPINS
 Encoder motor1_enc(D12,D11);        // encoder outputpins
 Encoder motor2_enc(D10,D9);
 
-//POTMETERS
-AnalogIn potright(A0);      // define the potmeter outputpins
-AnalogIn potleft(A1);
-
-// RESETBUTTON              
-DigitalIn   button(PTA4);      // defines the button used for a encoder reset
-int button_pressed = 0;
-
 int reference1 = 0;         // set the reference position of the encoders
 int reference2 = 0;
 
+
+// EXTRA INPUTS AND REQUIRED VARIABLES
+//POTMETERS
+    AnalogIn potright(A0);      // define the potmeter outputpins
+    AnalogIn potleft(A1);
+
+// RESETBUTTON              
+    DigitalIn   button(PTA4);      // defines the button used for a encoder reset
+    int button_pressed = 0;
+
+
+// SETPOINT SETTINGS
+// define storage variables for setpoint values
+    double c_setpoint1 = 0;
+    double c_setpoint2 = 0;
+// define the maximum rate of change for the setpoint (velocity)
+    double Vmax = 5;            // rad/sec
+
 // CONTROLLER SETTINGS
     // motor 1
     const double m1_Kp = 1;         // Proportional constant     
-    const double m1_Ki = 0;         // integration constant
+    const double m1_Ki = 1;         // integration constant
     const double m1_Kd = 0;         // differentiation constant
     // motor 2
     const double m2_Kp = 1;
@@ -52,14 +67,6 @@
     double m2_prev_err = 0;
 
 
-// SETPOINT SETTINGS
-// define storage variables for setpoint values
-    double c_setpoint1 = 0;
-    double c_setpoint2 = 0;
-// define the maximum rate of change for the setpoint (velocity)
-    double Vmax = 5;            // rad/sec
-
-
 //// FILTER VARIABLES
 // storage variables
         // differential action filter, same is used for both controllers
@@ -95,30 +102,34 @@
 {
     double offset = 0.5, gain = 2;                
     double potset = (input-offset)*gain;
+    if(potset < 0.1 && potset > -0.1)
+    { 
+        potset = 0;
+    }
     double setpoint = c_setpoint + potset * controlstep * Vmax ;
         c_setpoint = setpoint;
     return setpoint;
 }
 
 
-// This function takes the inputvalue and ensures it is between -1 and 1
+// This function takes the controller inputvalue and ensures it is between -1 and 1
 // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
 // I am aware it has unnecesary steps but it works.
-double outputlimiter (double output)
+double outputlimiter (double output, double limit)
 {
-    if(output>1)                                
+    if(output> limit)                                
     {
         output = 1;
     }
-    else if(output < 1 && output > 0)
+    else if(output < limit && output > 0)
     {
         output = output;
     }
-    else if(output > -1 && output < 0)
+    else if(output > -limit && output < 0)
     {
         output = output;
     }
-    else if(output < -1)
+    else if(output < -limit)
     {
         (output = -1);
     }
@@ -140,23 +151,24 @@
 
 // PID Controller given in sheets
 // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!)
-double PID( double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
+double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
 {
 // Proportional
 double P = Kp * e;
-
+pc.printf("P = %f ",P);
 // Derivative
 double e_derr = (e - e_prev)/Ts;
-e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2);
+pc.printf("D-actie %f",e_derr);
+// e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2);
 e_prev = e;
-double D = Kd* e_derr;
+// double D = Kd* e_derr;
 
 // Integral
 e_int = e_int + Ts * e;
 double I = e_int * Ki;
 
 // PID
-double output = P + I + D;
+double output = P + I ;// + D;
 return output;
 }
 
@@ -166,7 +178,7 @@
 double K_control(double error,double K)
 {
     double output = K*error;                            // controller output K*e
-    output = outputlimiter(output);             // limit the output to -1->1
+    output = outputlimiter(output,1);             // limit the output to -1->1
     return output;
 }
 
@@ -181,12 +193,15 @@
     double setpoint1 = setpoint_f(potright.read(), c_setpoint1);      // determine the setpoint that has been set by the inputsignal
     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
     double error1 = (setpoint1 - rads1);                       // determine the error (reference - position)
-    // scope.set(0,setpoint1);                                                                                                              // disabled because HIDScope is acting strangely
-    // scope.set(1,rads1);
-    // scope.send();
-    
+    scope.set(0,setpoint1);                                                                                                              // disabled because HIDScope is acting strangely
+    scope.set(1,rads1);
+    scope.send();   
     // double output1 = K_control(error1, m1_Kp);        // bereken de controller output voor motor 1(zie hierboven)
     double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
+    pc.printf("output 1 %f \n",output1);
+  
+    // ws best locatie om output te blokkeren als grenzen bereikt zijn, simpel if-loopje met rad1
+    
     if(output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
         motor1_rich.write(0);
         motor1_aan.write(output1);
@@ -218,13 +233,40 @@
 }
 
 
+//////////////////////////////////////////////////////////////////
+//////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
+////////////////////////////////////////////////////////////////
+
+void motor1_activate()
+{ 
+    motor1_go = true; 
+}
+ 
+void motor2_activate()
+{ 
+    motor2_go = true; 
+}
+
+
+
 int main()
 {
+    pc.baud(115200);
     // Ticker calling the primary functions. If neccessary use the ticker to change bools and execute in the main loop as shown in the sheets.
-    controller1.attach(&motor1_control, controlstep);
-    controller2.attach(&motor2_control, controlstep);
+    controller1.attach(&motor1_activate, controlstep);
+    controller2.attach(&motor2_activate, controlstep);
     while(true) 
     {
+        if(motor1_go)
+        {
+            motor1_go = false;
+            motor1_control();
+        }
+        if(motor2_go)
+        {
+            motor2_go = false;
+            motor2_control();
+        }
         if(button.read() == button_pressed)             // reset the encoder to reference position
         {
             motor1_enc.setPosition(reference1);