Diff: main.cpp

Revision:

9:c4fa72ffa1c2

Parent:

5:7e2c6d2235fe

Child:

10:a60b369c1711

--- a/main.cpp	Mon Oct 21 23:28:27 2019 +0000
+++ b/main.cpp	Tue Oct 22 22:37:32 2019 +0000
@@ -42,6 +42,8 @@
 double f=1/100;                     // Frequency
 const double Ts = f/10;            // Sampletime
 HIDScope scope(2);                  // Amount of HIDScope servers
+double positie_verschil_motor1;
+double positie_prev_motor1;
 
 // 3.3 BiQuad Filters **********************************************************
 static BiQuad notchfilter(9.97761e-01, -1.97095e+00, 9.97761e-01, -1.97095e+00, 9.95522e-01);
@@ -108,11 +110,22 @@
     // 4.1 Hidscope ****************************************************************
     void HIDScope() //voor HIDscope
     {
-        scope.set(0, Yref_motor1); //nog te definieren wat we willen weergeven
-        scope.set(1, positie_motor1); //nog te definieren wat we willen weergeven
+        scope.set(0, positie_motor1);
+       scope.set(1, positie_prev_motor1); //nog te definieren wat we willen weergeven
+       scope.set(2, positie_verschil_motor1); //nog te definieren wat we willen weergeven
+
         scope.send();   
     }
-
+    
+        double fencoder_motor1()                                    // bepaalt de positie van de motor
+        {
+            positie_motor1 = encoder_motor1.getPulses();                // haalt encoder waardes op
+            positie_verschil_motor1 =  (positie_motor1-positie_prev_motor1)/Ts;
+            positie_prev_motor1 = positie_motor1;
+            
+            return positie_motor1;                                 //geeft positie van motor
+        }
+  
     // 4.2a PID-Controller motor 1**************************************************
     double PID_controller_motor1()
     {
@@ -161,14 +174,30 @@
         return P_motor2; 
     }
     // 4.3 State-Machine *******************************************************
-
+        void motor1_calibration()
+        {
+            motor1DirectionPin=0; //direction of the motor
+            motor1=0.2;
+            wait(1);
+            while (abs(positie_verschil_motor1)>5)
+            {
+                motor1=0.2  ; 
+                pc.printf("\r\n Kalibratie werkt ");
+            }
+            motor1=0.0;  
+            pc.printf("\r\n Kalibratie werkt niet");
+        
+        }
+    
 //******************************************************************************
 // 5. Main Loop ****************************************************************
 //******************************************************************************
 
 void main_loop() { //Beginning of main_loop()
 //    pc.printf("main_loop is running succesfully \r\n"); //confirmation that main_loop is running (als je dit erin zet krijg je elke duizendste dit bericht. Dit is niet gewenst)
- 
+fencoder_motor1() ;
+
+
 // 5.1 Measure Analog and Digital input signals ********************************
 // 5.2 Run state-machine(s) ****************************************************
 // 5.3 Run controller(s) *******************************************************
@@ -194,10 +223,10 @@
     led_green.write(1);
     led_red.write(1);
     led_blue.write(0);
-    
-    ticker_mainloop.attach(&main_loop,0.001f); // change back to 0.001f //Run the function main_loop 1000 times per second       
-    ticker_hidscope.attach(&HIDScope, 0.1f); //Ticker for Hidscope, different frequency compared to motors
-    
+   
+    ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second       
+    ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
+    motor1_calibration();
 // 6.2 While loop in main function**********************************************   
     while (true) { } //Is not used but has to remain in the code!!