Diff: main.cpp

Revision:

14:725a608b6709

Parent:

13:934af23bf416

Child:

15:9061cf7db23e

--- a/main.cpp	Thu Oct 20 11:24:19 2016 +0000
+++ b/main.cpp	Thu Oct 20 12:31:42 2016 +0000
@@ -8,10 +8,10 @@
 //set pins
 DigitalIn encoder1A (D13); //Channel A van Encoder 1
 DigitalIn encoder1B (D12); //Channel B van Encoder 1
-DigitalIn encoder2A (D14); //Channel A van Encoder 2
-DigitalIn encoder2B (D15); //Channel B van Encoder 2
-DigitalOut led1 (D11); 
-DigitalOut led2 (D10);
+DigitalIn encoder2A (D11); //Channel A van Encoder 2, kan niet op D15
+DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14
+//DigitalOut led1 (D11); 
+//DigitalOut led2 (D10);
 AnalogIn potMeter1(A2);
 AnalogIn potMeter2(A1);
 DigitalOut motor1DirectionPin(D7);
@@ -40,8 +40,8 @@
  float motorValue2 = 0.0;
  
 //set constant or variable values
-int counts1 = 0;
-int counts2 = 0;
+//int counts1 = 0;
+//int counts2 = 0;
 int counts1Prev = 0;
 int counts2Prev = 0;
 double DerivativeCounts;
@@ -75,6 +75,9 @@
 //define encoder counts and degrees
 QEI Encoder1(D12, D13, NC, 32); // turns on encoder
 QEI Encoder2(D14, D15, NC, 32); // turns on encoder
+    int counts1 = Encoder1.getPulses();           // gives position of encoder 
+    int counts2 = Encoder2.getPulses();           // gives position of encoder 
+
 const int counts_per_revolution = 4200; //counts per motor axis revolution
 const int inverse_gear_ratio = 131;
 //const float motor_axial_resolution = counts_per_revolution/(2*PI);
@@ -145,16 +148,17 @@
     pc.baud(115200);
     pc.printf("dx:          %f \r\n", dx);
     pc.printf("dy:          %f \r\n", dy);
-    pc.printf("Encoder1:    %f \r\n", Encoder1Position);
-    pc.printf("Motor1:      %f \r\n", Motor1Position);
     pc.printf("q1:          %f \r\n", q1Out);
     pc.printf("q1_dot:      %f \r\n", q1_dotOut);
-    pc.printf("Encoder2:    %f \r\n", Encoder2Position);
-    pc.printf("Motor2:      %f \r\n", Motor2Position);
     pc.printf("q2:          %f \r\n", q2Out);
     pc.printf("q2_dot:      %f \r\n", q2_dotOut);
     
-    
+    pc.printf("Counts1:     %f \r\n", counts1);
+    pc.printf("Encoder1:    %f \r\n", Encoder1Position);
+    pc.printf("Motor1:      %f \r\n", Motor1Position);
+    pc.printf("Counts2:    %f \r\n", counts2);
+    pc.printf("Encoder2:    %f \r\n", Encoder2Position);
+    pc.printf("Motor2:      %f \r\n", Motor2Position);
     }
 
 void FeedForwardControl1(float q1_dot, float q2_dot, float &motorValue1Out, float &motorValue2Out){
@@ -229,11 +233,11 @@
         else motor1MagnitudePin = fabs(motorValue1);
     //control motor 2
     if (motorValue2 >=0) 
-        {motor2DirectionPin=cw;
+        {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
         //led1=1;
         //led2=0;
         }
-    else {motor2DirectionPin=ccw;
+    else {motor2DirectionPin=cw;    //action is ccw, due to faulty motor2DirectionPin (inverted)
         //led1=0;
         //led2=1;
         }
@@ -284,8 +288,8 @@
         if (MeasureTicker_go){
             MeasureTicker_go=false;
             MeasureAndControl();
-            counts1 = Encoder1.getPulses();           // gives position of encoder 
-            counts2 = Encoder2.getPulses();           // gives position of encoder 
+            //counts1 = Encoder1.getPulses();           // gives position of encoder 
+            //counts2 = Encoder2.getPulses();           // gives position of encoder 
             }
 /*
         if (BiQuadTicker_go){