Diff: main.cpp

Revision:

2:0954eb04bb4c

Parent:

1:d8bb72c9c019

Child:

3:93f4ab4a7339

Child:

4:8b1df22779a7

--- a/main.cpp	Mon Oct 03 13:39:28 2016 +0000
+++ b/main.cpp	Thu Oct 13 11:07:26 2016 +0000
@@ -1,38 +1,53 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "QEI.h"
-
+#include "math.h"
+#include "HIDScope.h"
+#include "Biquad.h"
+ 
+DigitalOut green(LED_GREEN);
 MODSERIAL pc(USBTX, USBRX);
 ///////////////////////////
 // Hardware initialiseren//
 ///////////////////////////
 //------------------------------------------------------------
-//--------------------All sensors---------------------------
+//--------------------All sensors-----------------------------
 //------------------------------------------------------------
+//--------------------Encoders--------------------------------
 
+QEI Encoder_L (D11, D10,NC, 32);  //D11 is poort A D10 is poort b
+QEI Encoder_R (D13, D12,NC, 32); //D 13 is poort A 12 is poort b
+
+double AnglePerPulse = 11.25;
+double Position_L = 0.0;
+double Position_R = 0.0;
+double Previous_Position_L = 0.0;
+double Previous_Position_R = 0.0;
 //--------------------Analog---------------------------------
-AnalogIn P_M_L(A0); //Motorspeed Left
-AnalogIn P_M_R(A1); //MotorSpeed Right
+AnalogIn P_M_L(A0); //Motorspeed Left control
+AnalogIn P_M_R(A1); //MotorSpeed Right control
 
-//-------------------Digital----------------------------------
-
+//-------------------Hidscope----------------------------------
+HIDScope scope(2); // Sending two sets of data
 
 //-------------------Interrupts-------------------------------
 InterruptIn Dir_L(D0); //Motor Dirrection left
 InterruptIn Dir_R(D1); //Motor Dirrection Right
 InterruptIn OnOff(SW3); //Motor On/off
 
-//------------------Ticker------------------------------------
-Ticker Measure;
+
+//------------------Tickers------------------------------------
+
+Ticker Measure; // ticker for data mesuaring
 //------------------------------------------------------------
 //--------------------All Motors----------------------------
 //------------------------------------------------------------
 
 //-------------------- Motor Links----------------------------
 DigitalOut M_L_D(D4); //Richting motor links
-PwmOut     M_L_S(D5); //Snelheid motor links
+PwmOut     M_L_S(D5); //Speed motor links
 DigitalOut M_R_D(D7); //Richting motor Rechts
-PwmOut     M_R_S(D6); //Snelheid motor Rechts
+PwmOut     M_R_S(D6); //Speed motor Rechts
 
 //--------------------------------------------------------------
 //-----------------------Functions------------------------------
@@ -41,6 +56,9 @@
 //-----------------------Interrupts-----------------------------
 volatile int Motor_Frequency = 1000;
 volatile bool Active = false;
+volatile float Speed_L= 0.0;
+volatile float Speed_R= 0.0;
+
 void Boot(){
   
     M_L_S.period(1.0/Motor_Frequency);
@@ -49,6 +67,9 @@
     M_R_S.period(1.0/Motor_Frequency);
     M_R_D = 0;
     M_R_S= 0.0;
+    
+    Encoder_L.reset();
+    Encoder_R.reset();
     }
 void Switch_Dirr_L(){ // Switching dirrection left motor (Every motor starts in clockwise dirrection)
      M_L_D = !M_L_D;
@@ -62,9 +83,26 @@
     }
     
 void Information(){
-    pc.printf("test");
+    
+//---------------------Motor Data-----------------------------------------------
+//Position in Radials:
+    Previous_Position_L = Position_L;
+    Previous_Position_R = Position_R;
+    Position_L = Encoder_L.getPulses()*AnglePerPulse+Previous_Position_L;
+    Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R;
+
+//Speed in RadPers second
+    Motor_Speed_L = Previous_Position_L - Position_R
+//---------------------Sending data---------------------------------------------
+    scope.set(0,M_L_S); //Sending motor left speed
+    scope.set(1,M_R_S); //sending random value
+    scope.send();
     }
-
+//-----------------------Encoders-----------------------------------------------
+int Get_Phase_L(int Phase){
+    Phase = Encoder_L.getCurrentState();    
+    return Phase;
+    }
 int main()
 {
 //---------------------Setting constants and system booting--------------------
@@ -72,23 +110,25 @@
     pc.printf("\r\n**BOARD RESET**\r\n");
     Boot();
 
+    
     Dir_L.fall(Switch_Dirr_L);
     Dir_R.fall(Switch_Dirr_R); 
     OnOff.fall(De_Activate);
-       
+    Measure.attach(Information, 0.01);     
     while (true) {  
        // control= pc.getc();  
        if (Active == true){
-            M_L_S = P_M_L.read()/5.0+0.1;
-            M_R_S = P_M_R.read()/5.0+0.1;
+            green = 0;
+            Speed_L =  P_M_L.read();//5.0+0.1;
+            M_L_S = Speed_L/5+0.1f;
+            Speed_R = P_M_R.read();//5.0+0.1;
+            M_R_S = Speed_R/5+0.1f ;
         } else {
             if(Active == false){
+            green = 1;
             M_L_S = 0;
             M_R_S = 0;
                 }
-        }
-        Measure.attach(Information, 2.0); 
-        //wait(1.0f);
-        
+        }       
     }
 }
\ No newline at end of file