Diff: main.cpp

Revision:

43:9115c84145c6

Parent:

42:3aa03b5cefb0

Child:

44:8872b20bc1bd

--- a/main.cpp	Fri Nov 03 03:52:30 2017 +0000
+++ b/main.cpp	Fri Nov 03 04:15:38 2017 +0000
@@ -686,44 +686,55 @@
 void SetMotor1() {
     //PIDcalculation1();
     //filter();
+    pc.printf("\n\rBegin SetMotor1\r");
     while (angle1<setpoint1 || angle1>setpoint1 || angle2<setpoint2 || angle2>setpoint2) {
+        pc.printf("\n\rlaten we dit dan 0 noemen\r");
         encoders();
-        float count1;
-        float count2;
+        double count1;
+        double count2;
         angle1 += (0.0981 * count1);
         angle2 += (0.0981 * count2);
         if (angle1<setpoint1 && angle2<setpoint2) {
+        pc.printf("\n\rangle1<setpoint1 && angle2<setpoint2\r");
         motor1direction = 1; // counterclockwise rotation
         motor2direction = 1;
         }
         else if (angle1>setpoint1 && angle2<setpoint2) {
+        pc.printf("\n\rangle1>setpoint1 && angle2<setpoint2\r");
         motor1direction = 0; // clockwise rotation
         motor2direction = 1;
         }
         else if (angle1<setpoint1 && angle2>setpoint2) {
+        pc.printf("\n\rangle1<setpoint1 && angle2>setpoint2\r");
         motor1direction = 1;
         motor2direction = 0;
         }
         else if (angle1>setpoint1 && angle2>setpoint2) {
+        pc.printf("\n\rangle1>setpoint1 && angle2>setpoint2\r");
         motor1direction = 0;
         motor2direction = 0;
         }
-        if ((angle1<setpoint1 || angle1>setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
+        if ((angle1<setpoint1 || angle1>setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {     //1
+        pc.printf("\n\rdit noem ik maar 1\r");
         motor1pwm = 0.2;
         motor2pwm = 0.2;
         }
         else if ((angle1<setpoint1 || angle1>setpoint1) && (angle2 == setpoint2)) {
+        pc.printf("\n\rdit noem ik maar 2\r");
         motor1pwm = 0.2;
         motor2pwm = 0;
         }
         else if ((angle1 == setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
+        pc.printf("\n\rdit noem ik maar 3\r");
         motor1pwm = 0;
         motor2pwm = 0.2;
         }
         else if ((angle1 == setpoint1) && (angle2 == setpoint2)){
+        pc.printf("\n\rdit noem ik maar 4\r");
         motor1pwm = 0;
         motor2pwm = 0;
         }
+    pc.printf("\n\rend SetMotor1\r");
     }
 }
 /*
@@ -756,7 +767,8 @@
 */
     
 int main(){   
-    pc.baud(115200);
+ // pc.baud(115200);
+    pc.printf("\n\rMAIN START\r");
     main_timer.attach(&filter, 0.001); // set frequency for the filters at 1000Hz
     max_read1.attach(&get_max1, 2); // set the frequency of the calibration loop at 0.5Hz
     max_read3.attach(&get_max3, 2);
@@ -764,5 +776,6 @@
     Motorcontrol.attach(&SetMotor1,0.1);
     //PIDtimer.attach(&PIDcalculation1, 0.005);
     //PIDtimer.attach(&PIDcalculation2, 0.005);
+    pc.printf("\n\rMAIN END!!!");
     while(1) {}
 }
\ No newline at end of file