Diff: actuators.cpp

Revision:

82:4cc8f9ad3fec

Parent:

81:71e7e98deb2c

Child:

83:8fa05f53fc73

--- a/actuators.cpp	Tue Oct 20 14:25:31 2015 +0200
+++ b/actuators.cpp	Tue Oct 20 14:30:54 2015 +0200
@@ -133,14 +133,14 @@
     motor1PID.Compute(); // calculate PID outputs, output changes automatically
     motor2PID.Compute();
 // write new values to motor's
-    if (motor1SetSpeed >= 0 ){ // CCW rotation 
+    if (motor1SetSpeed > 0 ){ // CCW rotation 
         direction1 = false;
         motor1PID.SetDirection(DIRECT); // increase in input > increase in output
     }else{
         direction1 = true; // CW rotation
         motor1PID.SetDirection(REVERSE); // increase in input > decrease in output
     }
-    if (motor2SetSpeed >= 0 ){ // CCW rotation 
+    if (motor2SetSpeed > 0 ){ // CCW rotation 
         direction2 = false;
         motor2PID.SetDirection(DIRECT);
     }else{
@@ -167,36 +167,36 @@
 }
 
 void calibrateMotors(){
-    // bool calibrating1 = true;
-    // bool calibrating2 = true;
-    // float motorCalSpeed = 0.1; // %PWM
-    // motor1Dir.write(false);
-    // motor2Dir.write(false);
-    // // move motors CCW until they reach outer most position
-    // // setmotorspeed contant
-    // // first move motor 1 using PID
-    // // wait until motor hits pin
-    // // switch direction
-    // // set motor speed1 0
-    // // repeat for motor 2
-    // while (calibrating1 || calibrating2){
-    //     if(motor1AnglePin == 0){
-    //         motor1.write(0.3f);
-    //     }else{
-    //         motor1.write(0);
-    //         calibrating1 = false;
-    //     }
-    //     if(motor2AnglePin == 0){
-    //         motor2.write(0.3f);
-    //     }else{
-    //         motor2.write(0);
-    //         calibrating2 = false;
-    //     }
-    //     wait(0.2f);
-    // }
-    // // set the encoder values for angle.
-    // encoder1.setValue(0);
-    // encoder2.setValue(0);
+    bool calibrating1 = true;
+    bool calibrating2 = true;
+    float motorCalSpeed = 0.1; // %PWM
+    motor1Dir.write(false);
+    motor2Dir.write(false);
+    // move motors CCW until they reach outer most position
+    // setmotorspeed contant
+    // first move motor 1 using PID
+    // wait until motor hits pin
+    // switch direction
+    // set motor speed1 0
+    // repeat for motor 2
+    while (calibrating1 || calibrating2){
+        if(motor1AnglePin == 0){
+            motor1.write(0.3f);
+        }else{
+            motor1.write(0);
+            calibrating1 = false;
+        }
+        if(motor2AnglePin == 0){
+            motor2.write(0.3f);
+        }else{
+            motor2.write(0);
+            calibrating2 = false;
+        }
+        wait(0.2f);
+    }
+    // set the encoder values for angle.
+    encoder1.setValue(0);
+    encoder2.setValue(0);
     safetyOn = true; // turn safety on after callibration
 }