Diff: main.cpp

Revision:

2:47b7491935dc

Parent:

1:2ec710725db2

--- a/main.cpp	Fri Sep 28 09:58:57 2018 +0000
+++ b/main.cpp	Mon Oct 15 10:56:12 2018 +0000
@@ -1,36 +1,76 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include "QEI.h"
 
-//AnalogIn pot1(A1);
-//AnalogIn pot2(A2);
-DigitalIn encoder(D8);
+AnalogIn pot1(A1);
+AnalogIn pot2(A2);
 PwmOut pwmpin1(D5);
 PwmOut pwmpin2(D6);
-DigitalOut directionpin(D4);
+PwmOut pwmpin3(D12);
+DigitalOut directionpin1(D4);
 DigitalOut directionpin2(D7);
+DigitalOut directionpin3(D13);
+QEI encoder1 (D11, D10, NC, 8400, QEI::X4_ENCODING);
+QEI encoder2 (D9, D8, NC, 8400, QEI::X4_ENCODING);
+QEI encoder3 (D3, D2, NC, 8400, QEI::X4_ENCODING);
 MODSERIAL pc(USBTX, USBRX);
 
+// ALS DE MOTOR GEK GEDRAG VERTOONT. ZORG ER VOOR DAT PINS D4-7 NIET WORDEN GEBRUIKT. DEZE ZIJN AL IN GEBURIK!!!!
+
+
+// INITIALIZING ENCODER
+int steps_per_rev = 8400;
+
+float pulsesToDegrees(float pulses)
+    {
+        return((pulses/steps_per_rev)*360);
+        }
+
 int main()
 {
         pc.baud(9600);
-    
+        pc.printf("hello world\n");
+   
+// INITIZALIZING ROTATION OF MOTOR  
     pwmpin1.period_us(60); //60 microsecondsPWM period, 16.7 kHz
     pwmpin2.period_us(60);
-   //float ain1;
-   //float ain2;
+    pwmpin3.period_us(60);
+      
     
     while(true){
-       // ain1 = pot1.read();
-        // ain2 = pot2.read();
+           
+float pot_control1 = 2*pot1.read()-1;
+float pot_control2 = 2*pot2.read()-1;
         
-        float u = -0.9f; //determineusefulvalue, -0.3f is justanexample
-        directionpin= u > 0.0f; //eithertrueor false
+        
+// CHANGE DIRECTION AND SPEED
+        float u = pot_control1; //determineusefulvalue, -0.3f is justanexample
+        directionpin1= u > 0.0f; //eithertrueor false
         pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value
-        pwmpin2= fabs(u);
+        
+        float k = pot_control2; //determineusefulvalue, -0.3f is justanexample
+        directionpin2= k > 0.0f; //eithertrueor false
+        pwmpin2= fabs(k);
+        
+        float q = k; //determineusefulvalue, -0.3f is justanexample
+        directionpin3= q > 0.0f; //eithertrueor false
+        pwmpin3= fabs(q);
+        
+        
+// ENCODER READOUT
         
+         
+        float alpha = pulsesToDegrees(encoder1.getPulses());
+        pc.printf("alpha: %f       ", alpha); 
+        
+        
+        float beta = pulsesToDegrees(encoder2.getPulses());
+        pc.printf("beta: %f        ", beta); 
+        
+        float gamma = pulsesToDegrees(encoder3.getPulses());
+        pc.printf("gamma: %f\n\r", gamma);
+            
         wait(0.5f);
         
-        printf("Motorsnelheid %i \n \r", encoder); 
-        
     }
 }
\ No newline at end of file