Diff: main.cpp

Revision:

2:2c32825bb327

Parent:

1:e10ae03926e6

Child:

3:fe052c67c840

--- a/main.cpp	Mon Oct 29 07:24:56 2018 +0000
+++ b/main.cpp	Mon Oct 29 10:36:47 2018 +0000
@@ -13,19 +13,20 @@
 DigitalOut directionpin1(D7);
 QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
 Ticker ref_rot;
-Ticker show_counts; 
+Ticker show_counts;
 Ticker Scope_Data;
 MODSERIAL pc(USBTX, USBRX);
-HIDScope scope(2);
+//HIDScope scope(2);
 
 // INITIALIZING GLOBAl VALUES
-double Kp = 50; //200
-double Ki = 0.5;   //1
-double Kd = 10; //200
+double Kp = 1; //200 , 50
+double Ki = 0;   //1, 0.5
+double Kd = 0; //200, 10
 double Ts = 0.1; // Sample time in seconds
-volatile double reference_rotation = 0; //define as radians
+volatile double reference_rotation = 3.14f; //define as radians
 double motor_position;
 bool AlwaysTrue;
+double PI = 3.141592653589793238462;
 
 //----------- FUNCTIONS ----------------------
 
@@ -33,25 +34,25 @@
 // PID CONTROLLER FUNCTION
 double PID_controller(double error)
 {
-  static double error_integral = 0;
-  static double error_prev = error; // initialization with this value only done once!
-  static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    static double error_integral = 0;
+    static double error_prev = error; // initialization with this value only done once!
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
 
-  // Proportional part:
-  double u_k = Kp * error;
+    // Proportional part:
+    double u_k = Kp * error;
 
-  // Integral part
-  error_integral = error_integral + error * Ts;
-  double u_i = Ki * error_integral;
+    // Integral part
+    error_integral = error_integral + error * Ts;
+    double u_i = Ki * error_integral;
 
-  // Derivative part
-  double error_derivative = (error - error_prev)/Ts;
-  double filtered_error_derivative = LowPassFilter.step(error_derivative);
-  double u_d = Kd * filtered_error_derivative;
-  error_prev = error;
+    // Derivative part
+    double error_derivative = (error - error_prev)/Ts;
+    double filtered_error_derivative = LowPassFilter.step(error_derivative);
+    double u_d = Kd * filtered_error_derivative;
+    error_prev = error;
 
-  // Sum all parts and return it
-  return u_k + u_i + u_d;   
+    // Sum all parts and return it
+    return u_k + u_i + u_d;
 }
 
 
@@ -59,7 +60,7 @@
 void moter_control(double u)
 {
     directionpin1= u > 0.0f; //eithertrueor false
-    pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value            
+    pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
 }
 
 
@@ -76,54 +77,64 @@
 void PrintFlag()
 {
     AlwaysTrue = true;
-    }
-    
+}
+/*
 // HIDSCOPE
 void ScopeData()
 {
     double y = encoder1.getPulses();
     scope.set(0, y);
     scope.send();
-    }
-
+}
+*/
 //------------------ EXECUTION ---------------
 
 int main()
 {
-  // INITIALIZING
-  
-  pwmpin1.period_us(60);
-  pc.printf("test");
-  pc.baud(9600);
-  //show_counts.attach(PrintFlag, 0.2);
-  ref_rot.attach(Motor_mover, 0.01);
-  Scope_Data.attach(ScopeData, 0.1);
- 
- // DEFININING VARIABLES
-            //float reference;
-            float foo[17];
-            float length = 9.0f;
+    // INITIALIZING
+
+    pwmpin1.period_us(60);
+    pc.printf("test");
+    pc.baud(9600);
+    //show_counts.attach(PrintFlag, 0.2);
+    ref_rot.attach(Motor_mover, 0.01);
+    //Scope_Data.attach(ScopeData, 0.1);
+
+// DEFININING VARIABLES
+    //float reference;
+    float foo[17];
+    float length = 9.0f;
+    int a;
+    int q;
+
+while (true) {
+
+    // EXECTURION  IN MAIN
+        // SIN ALS INPUT GEVEN
+        if (button2 == false){
+            wait (0.2f);
+        for (q=0; q<4; q++){
             float b = 8.0f;
-            int a;
- 
- 
-  
-  // EXECTURION  IN MAIN
-        for (a=0; a<10; a++){
-            foo[a] = 3.14f/length*(a);
-            printf("%f\n\r", foo[a]);
+            for (a=0; a<10; a++){
+                foo[a] = 3.14f/length*(a);
+                //printf("%f\n\r", foo[a]);
+                reference_rotation = foo[a];
+                wait(0.1f);
+                }
+            for(a=10; a<18; a++){
+                foo[a] = 3.14f/length*b;
+                //printf("%f\n\r", foo[a]);
+                reference_rotation = foo[a];
+                b = b-1.0f;
+                wait(0.1f);
+                }
             }
-        for(a=10; a<18; a++){
-            foo[a] = 3.14f/length*b;
-            printf("%f\n\r", foo[a]);
-            reference = foo[a];
-            b = b-1.0f;
-            }        
-  
-  
-    
-    while (true) {
-        /*
+
+
+
+
+
+        /* //IETS PRINTEN OP JE TERMINAL
         PrintFlag();
         while (AlwaysTrue){
             float q = encoder1.getPulses();
@@ -132,19 +143,10 @@
             wait(0.2f);
             }
             */
-           // sin input 
-           /*
-           if (button2 == false){
-            wait (0.2f);
-            for (b=0; b<4; b++){
-                  for(a=0; a<18; a++){
-                    reference_rotation = foo[a];
-                    wait(0.1f);
-                     }
-                    }
-                }
-                */
-            }
             
             
+
     }
+
+}
+}