Diff: main.cpp

Revision:

39:c15f84af49fd

Parent:

38:32c7e08bb281

Child:

40:c40bd1a84e7c

--- a/main.cpp	Fri Nov 03 02:19:10 2017 +0000
+++ b/main.cpp	Fri Nov 03 02:32:43 2017 +0000
@@ -665,32 +665,54 @@
 // check to see if motor1 needs to be activated
 void SetMotor1() {
     //PIDcalculation1();
-    //filter();
-    count1 = Encoder1.getPulses();
-    angle1 += (0.0981 * count1);
-    while (angle1<setpoint1 || angle1>setpoint1) {
-        if (angle1<setpoint1){
+    filter();
+    while (angle1<setpoint1 || angle1>setpoint1 || angle2<setpoint1 || angle2>setpoint2) {
+        count1 = Encoder1.getPulses();
+        count2 = Encoder2.getPulses();
+        angle1 += (0.0981 * count1);
+        angle2 += (0.0981 * count2);
+        if (angle1<setpoint1 && angle2<setpoint2) {
         motor1direction = 0; // counterclockwise rotation
+        motor2direction = 0;
+        }
+        else if (angle1>setpoint1 && angle2<setpoint2) {
+        motor1direction = 1; // clockwise rotation
+        motor2direction = 0;
+        }
+        else if (angle1<setpoint1 && angle2>setpoint2) {
+        motor1direction = 0;
+        motor2direction = 1;
         }
-        else if (angle1>setpoint1){
-        motor1direction = 1; // clockwise rotation
+        else if (angle1>setpoint1 && angle2>setpoint2) {
+        motor1direction = 1;
+        motor2direction = 1;
+        }
+        if ((angle1<setpoint1 || angle1>setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
+        motor1pwm = 0.2;
+        motor2pwm = 0.2;
         }
-        if (angle1<setpoint1 || angle1>setpoint1) {
-        motor1pwm = 0.5;
+        else if ((angle1<setpoint1 || angle1>setpoint1) && (angle2 == setpoint2)) {
+        motor1pwm = 0.2;
+        motor2pwm = 0;
         }
-        else if (angle1 == setpoint1){
+        else if ((angle1 == setpoint1) && (angle2<setpoint2 || angle2>setpoint2)) {
         motor1pwm = 0;
+        motor2pwm = 0.2;
+        }
+        else if ((angle1 == setpoint1) && (angle2 == setpoint2)){
+        motor1pwm = 0;
+        motor2pwm = 0;
         }
     }
 }
-
+/*
 // check if motor1 needs to be activated
 void SetMotor2() {
-    //filter();
+    filter();
     //PIDcalculation2();
-    count2 = Encoder2.getPulses();
-    angle2 += (0.0981 * count2);
     while (angle2<setpoint2 || angle2>setpoint2) {
+        count2 = Encoder2.getPulses();
+        angle2 += (0.0981 * count2);
         if (angle2<setpoint2){
         motor1direction = 0; // counterclockwise rotation
         }
@@ -705,10 +727,10 @@
         }
     }
 }
-
+*/
 void MeasureAndControl(void) {
     SetMotor1();
-    SetMotor2();
+    //SetMotor2();
 }