Diff: main.cpp

Revision:

25:c719346df3cd

Parent:

24:b9dd6cf5c366

Child:

26:8fc7e88603f8

diff -r b9dd6cf5c366 -r c719346df3cd main.cpp
--- a/main.cpp	Tue Oct 30 15:02:26 2018 +0000
+++ b/main.cpp	Tue Oct 30 21:27:35 2018 +0000
@@ -1,39 +1,70 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "BiQuad.h"
+#include "HIDScope.h"
+#include <math.h>
 
-AnalogIn potmetervalue1     (A1);
-AnalogIn potmetervalue2     (A2);
-
-DigitalIn button2           (D10);                  //Let op, is deze niet bezet?
+//ATTENTION:    set mBed to version 151
+//              set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
+//              set MODSERIAL to version 44
+//              set HIDScope to version 7
+//              set biquadFilter to version 7
 
 InterruptIn encoderA        (D9);
 InterruptIn encoderB        (D8);
 
 DigitalOut directionpin1    (D7);
 DigitalOut directionpin2    (D4);
+
+DigitalIn  button2          (D10);
+
 PwmOut pwmpin1              (D6);
 PwmOut pwmpin2              (D5);
 
 
 MODSERIAL pc(USBTX, USBRX);
+//HIDScope    scope( 3 );                             //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
 
 
 //Global variables
-int encoder = 0;                //Start value encoder
-int T = 0.002f;                 //Ticker period
+int encoder = 1;                //Start value encoder
 
-double Kp = 17.5;
+double T = 0.01f;                 //Ticker period
+
+double Kp = 17.5;              //Motor 1
 double Ki = 1.02;
 double Kd = 23.2;
+double PI = 3.14159;
 
-double err = 0;
+//double err = 0;
+
+volatile double reference_rotation = PI;   //pi , in radians
+double encoder_radians;
+
+//double u_total = 0;
+
+double pi=3.14159;
+double i=0;
 
 //Tickers
-Ticker function_tick;
+Ticker encoder_tick;
+Ticker engine_tick;
    
 
 //Functions
+    
+/*
+void HIDScope_sample()
+{
+    scope.set(0,sinout);
+    scope.set(1,encoder);                   //alles in array?
+    scope.set(2,err);
+    
+    scope.send();
+}
+*/
+    
+
 void encoderA_rise()       
 {
     if(encoderB==false)
@@ -90,7 +121,7 @@
     encoderB.fall(&encoderB_fall);
 }
 
-double PID_controller()
+double PID_controller(double err)
 {
   static double err_integral = 0;
   static double err_prev = err; // initialization with this value only done once!
@@ -111,31 +142,84 @@
   err_prev = err;
 
   // Sum all parts and return it
-  return u_k + u_i + u_d;   
+  return u_k + u_i + u_d;  
 }
 
-void call_all_functions()
+void start_your_engines(double u)
 {
-    PID_controller();
-    encoder_count();
+    if(encoder<5000 && encoder>-5000)              //limieten van de translatie 11760 encoder counts
+    {
+         pwmpin1 = fabs(u);               //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+         directionpin1.write(u > 0.0f);
+    }
+    else
+    {
+        pwmpin1 = 0;
+    }
+}  
+
+void engine_control()
+{
+    encoder_radians = encoder*(2*PI)/8400;
+    double err = reference_rotation - encoder_radians;
+    double u = PID_controller(err);                             //PID controller function call
+    start_your_engines(u);                                      //Call start_your_engines function
 }
 
 // Main function start.
 
 int main()
 { 
-    pc.baud(115200);
-    pc.printf("hello\n\r");
-    pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz 
+        pc.baud(115200);
+        pc.printf("hello\n\r");
+        pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz 
+
+        encoder_tick.attach(&encoder_count,T);
+        engine_tick.attach(&engine_control,T);
+        //HIDScope ticker toevoegen
+        
+// DEFININING VARIABLES
+
+//float reference;
+    float foo[17];
+    float length = 9.0f;
+    int a;
+    int q;
+
+while (true) 
+{
 
-    function_tick.attach(&call_all_functions,T);
-    
-    
-    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;
+                    
+                    for (a=0; a<10; a++)
+                    {
+                        foo[a] = 3.14f/length*(a);
+                        //printf("%f\n\r", foo[a]);
+                        reference_rotation = foo[a];
+                        wait(0.2f);
+                    }
+            
+                    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.2f);
+                    }
+            }
+
+            reference_rotation = 0;
         
+        }   
 
-                
-    }
 }
-
+}