Diff: main.cpp

Revision:

7:a3e073b8dd29

Parent:

6:79e42e1b87cb

Child:

8:b4cf0a68e37f

--- a/main.cpp	Tue Oct 22 00:42:37 2019 +0000
+++ b/main.cpp	Tue Oct 22 14:52:02 2019 +0000
@@ -32,7 +32,7 @@
 //*****************************************************************************
 // 3.1 Tickers *****************************************************************
 Ticker ticker_mainloop;      // The ticker which runs the mainloop
-Ticker ticker_hidscope; // The ticker which sends data to the HIDScope server
+Ticker hidscope_ticker; // The ticker which sends data to the HIDScope server
 
 // 3.2 General variables *******************************************************
 
@@ -41,8 +41,8 @@
 QEI encoder_motor2(D10,D11,NC,64); // Defines encoder for motor 1
 double f=1/100;                     // Frequency
 const double Ts = f/1000;            // Sampletime
-HIDScope scope(2);                  // Amount of HIDScope servers
-
+HIDScope scope(1);                  // Amount of HIDScope servers
+int test = 1;
 // 3.3 BiQuad Filters **********************************************************
 static BiQuad notchfilter(9.97761e-01, -1.97095e+00, 9.97761e-01, -1.97095e+00, 9.95522e-01);
 static BiQuad highfilter(9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01);   
@@ -72,7 +72,7 @@
  
 // 3.5 PID variabelen ***************************************************************   
     //3.5a PID-controller motor 1
-    double counts_per_rad_motor1 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
+    double counts_per_rad_motor1 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~~668.45 counts per rad
     static double error_integral_motor1 = 0;
     double Yref_motor1;                                 
     double kp_motor1;
@@ -80,12 +80,14 @@
     double Kd_motor1;
 
     double positie_motor1;                              //counts encoder
+    double positie_verschil_motor1;
+    double positie_prev_motor1 = 200;
     double error1_motor1;
     double error1_prev_motor1;
     double error1_derivative_motor1;
     double error1_derivative_filtered_motor1;
     double P_motor1;
-    
+
     //3.5b PID-controller motor 2
     double counts_per_rad_motor2 = (131.25*32)/(2*3.14159265359); // (gear ratio * counts per revolution) / (2* pi) = ~668.45 counts per rad
     static double error_integral_motor2 = 0;
@@ -95,6 +97,8 @@
     double Kd_motor2;
 
     double positie_motor2;                              //counts encoder
+    double positie_verschil_motor2;
+    double positie_prev_motor2;
     double error1_motor2;
     double error1_prev_motor2;
     double error1_derivative_motor2;
@@ -108,11 +112,25 @@
     // 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, test);
+//        scope.set(0, positie_motor1); //nog te definieren wat we willen weergeven
+//        scope.set(1, positie_verschil_motor1); //nog te definieren wat we willen weergeven
+//        scope.set(2, positie_prev_motor1);
         scope.send();   
     }
 
+    // 4.2 Encoders ****************************************************************
+        // 4.2a Encoder motor1 ****************************************************************
+        double Fun_encoder_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.2b Encoder motor2 ****************************************************************
+
     // 4.2a PID-Controller motor 1**************************************************
     double PID_controller_motor1()
     {
@@ -160,7 +178,27 @@
 
         return P_motor2; 
     }
-    // 4.3 State-Machine *******************************************************
+    
+    // 4.4 motor_calibration *******************************************************
+        // 4.4a Motor1_calibration *******************************************************
+
+        void motor1_calibration()
+        {
+            motor1DirectionPin=0; //direction of the motor
+            motor1=0.2;
+            wait(0.5);
+            while (abs(positie_verschil_motor1)>100)
+            {
+                motor1=0.2  ;
+                pc.printf("\r\n While loop werkt");  
+            }
+            motor1=0.0;
+            pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0");  pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0"); pc.printf("\r\n motor 0"); 
+            //State=EMGCalibration;
+        
+        }
+        
+    // 4.6 State-Machine *******************************************************
     void state_machine()
     {
         if (sw2==0) {State = EmergencyMode;}
@@ -175,8 +213,8 @@
                 
             case MotorCalibration: 
                 pc.printf("\r\n State: MotorCalibration");
-                
-                State=EMGCalibration;
+                motor1_calibration();
+                //When motor1_calibration is done it gives State=EMGCalibration;
                 break;
                 
             case EMGCalibration: 
@@ -201,7 +239,7 @@
                 
             case Operating:
                 pc.printf("\r\n State: Operating");
-                motor1=1;
+                //motor1=1;
                 break;
                 
             case Idle:
@@ -235,14 +273,15 @@
     
             }
     }
-
+    
 //******************************************************************************
 // 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)
- 
+Fun_encoder_motor1()  ;
+
 // 5.1 Measure Analog and Digital input signals ********************************
 // 5.2 Run state-machine(s) ****************************************************
     state_machine();
@@ -271,7 +310,7 @@
     led_blue.write(1);  //Led off
     
     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
+    hidscope_ticker.attach(&HIDScope, 0.01f); //Ticker for Hidscope, different frequency compared to motors
     
 // 6.2 While loop in main function**********************************************   
     while (true) { } //Is not used but has to remain in the code!!