Diff: main.cpp

Revision:

0:a2db8cc5d5df

Child:

1:d8bb72c9c019

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 03 13:14:39 2016 +0000
@@ -0,0 +1,73 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+
+MODSERIAL pc(USBTX, USBRX);
+///////////////////////////
+// Hardware initialiseren//
+///////////////////////////
+//------------------------------------------------------------
+//--------------------All sensors---------------------------
+//------------------------------------------------------------
+
+//--------------------Analog---------------------------------
+AnalogIn P_M_L(A0); //Motorspeed Left
+AnalogIn P_M_R(A1); //MotorSpeed Right
+
+//-------------------Digital----------------------------------
+
+
+//-------------------Interrupts-------------------------------
+InterruptIn Dir_L(D0); //Motor Dirrection left
+InterruptIn Dir_R(D1); //Motor Dirrection Right
+//------------------------------------------------------------
+//--------------------All Motors----------------------------
+//------------------------------------------------------------
+
+//-------------------- Motor Links----------------------------
+DigitalOut M_L_D(D4); //Richting motor links
+PwmOut     M_L_S(D5); //Snelheid motor links
+DigitalOut M_R_D(D7); //Richting motor Rechts
+PwmOut     M_R_S(D6); //Snelheid motor Rechts
+
+//--------------------------------------------------------------
+//-----------------------Functions------------------------------
+//--------------------------------------------------------------
+
+//-----------------------Interrupts-----------------------------
+volatile int Motor_Frequency = 1000;
+void Boot(){
+  
+    M_L_S.period(1.0/Motor_Frequency);
+    M_L_D = 1;
+    M_L_S = 0.0;
+    M_R_S.period(1.0/Motor_Frequency);
+    M_R_D = 0;
+    M_R_S= 0.0;
+    }
+void Switch_Dirr_L(){ // Switching dirrection left motor (Every motor starts in clockwise dirrection)
+     M_L_D = !M_L_D;
+    }
+    
+void Switch_Dirr_R(){ // Switching dirrection Right motor
+    M_R_D = !M_R_D;
+    }
+
+int main()
+{
+//---------------------Setting constants and system booting--------------------
+    pc.baud(115200);
+    pc.printf("\r\n**BOARD RESET**\r\n");
+    Boot();
+
+    Dir_L.fall(Switch_Dirr_L);
+    Dir_R.fall(Switch_Dirr_R); 
+       
+    while (true) {  
+       // control= pc.getc();  
+        M_L_S = P_M_L.read()/5.0+0.1;
+        M_R_S = P_M_R.read()/5.0+0.1;
+        //wait(1.0f);
+        pc.printf("%f",M_L_S);
+    }
+}
\ No newline at end of file