Diff: main.cpp

Revision:

24:f9dccbc1fc7e

Parent:

22:2a560f0f1671

Child:

25:039e2b6429ad

--- a/main.cpp	Wed Oct 31 19:42:55 2018 +0000
+++ b/main.cpp	Wed Oct 31 21:11:28 2018 +0000
@@ -27,6 +27,7 @@
 volatile double referencePosition2 = 0.0;
 volatile double motorValue1 = 0.01;
 volatile double motorValue2 = 0.01;
+volatile double errorM1 = 0.0;
 volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
 volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
 volatile double Kd = 0.0;
@@ -37,14 +38,14 @@
 double GetReferencePosition1()
 {
     double potMeter1In = potMeter1.read();
-    referencePosition1 = 4.0*3.14*potMeter1In - 2.0*3.14 ; // Reference value y, scaled to -2 to 2 revolutions (or 0 to 100 pi) WAAROM?
+    referencePosition1 = 8.0*3.14*potMeter1In - 4.0*3.14 ; // Reference value y, scaled to -4 to 4 revolutions
     return referencePosition1;
 }
 
 double GetReferencePosition2()
 {
     double potMeter2In = potMeter2.read();
-    referencePosition2 = 4.0*3.14*potMeter2In - 2.0*3.14 ;
+    referencePosition2 = 8.0*3.14*potMeter2In - 4.0*3.14 ;
     return referencePosition2;
 }
 
@@ -108,10 +109,12 @@
     // bits for motor 1. Positive value makes motor rotating
     // clockwise. motorValues outside range are truncated to
     // within range
+    // 0 = clockwise motor direction
+    // 1 = counterclockwise motor direction
     if (motorValue1 >=0) {
+        motor1DirectionPin=0;
+    } else {
         motor1DirectionPin=1;
-    } else {
-        motor1DirectionPin=0;
     }
     if (fabs(motorValue1)>1) {
         motor1MagnitudePin = 1;
@@ -126,6 +129,8 @@
     // bits for motor 1. Positive value makes motor rotating
     // clockwise. motorValues outside range are truncated to
     // within range
+    // 0 = counterclockwise motor direction
+    // 1 = clockwise motor direction
     if (motorValue2 >=0) {
         motor2DirectionPin=1;
     } else {
@@ -146,6 +151,7 @@
     // a simple Feedback controller. Call this from a Ticker.
     referencePosition1 = GetReferencePosition1();
     measuredPosition1 = GetMeasuredPosition1();
+    errorM1 = referencePosition1 - measuredPosition1;
     motorValue1 = FeedbackControl1(referencePosition1 - measuredPosition1);
     SetMotor1(motorValue1);
 }
@@ -164,10 +170,9 @@
 {
     pc.baud (115200);
     pc.printf("Referenceposition POT1 =  %f \t Referenceposition POT2 = %f \r\n", referencePosition1, referencePosition2);
-    
-    pc.printf("Measured position 1 %f \r\n", measuredPosition1);
-    pc.printf("Measured position 2 %f \r\n", measuredPosition2);
-    pc.printf("Motorvalue M1 = %f \t Motorvalue M2 = %f \r\n", motorValue1, motorValue2);
+    pc.printf("Measured position 1 = %f \t Measured position 2 = %f \r\n", measuredPosition1, measuredPosition2);
+    pc.printf("Error M1 = %f \r\n", errorM1);
+    pc.printf("Motorvalue M1 = %f \r\n", motorValue1);
     //pc.printf("Proportional gain %f \r\n", Kp);
     //pc.printf("Integral gain %f \r\n", Ki);
     //pc.printf("Derivative gain %f \r\n", Kd);