Diff: actuators.cpp

Revision:

84:df770ec4df61

Parent:

83:8fa05f53fc73

Child:

85:03dc24043236

Child:

91:6bbbbc6643c8

--- a/actuators.cpp	Tue Oct 20 14:58:55 2015 +0200
+++ b/actuators.cpp	Tue Oct 20 15:07:51 2015 +0200
@@ -133,13 +133,15 @@
     motor1PID.Compute(); // calculate PID outputs, output changes automatically
     motor2PID.Compute();
 // write new values to motor's
+    if (motor1SetSpeed=0){direction1=!direction1;} // change direction to cause braking.
     if (motor1SetSpeed > 0 ){ // CCW rotation 
         direction1 = false;
-        motor1PID.SetOutputLimits(0,1);
+        motor1PID.SetOutputLimits(0,1); // change pid output direction
     }else{
         direction1 = true; // CW rotation
         motor1PID.SetOutputLimits(-1,0);
     }
+    if (motor2SetSpeed=0){direction2=!direction2;} // change direction to cause braking.
     if (motor2SetSpeed > 0 ){ // CCW rotation 
         direction2 = false;
         motor2PID.SetOutputLimits(0,1);
@@ -167,36 +169,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
 }
 
@@ -205,6 +207,8 @@
     if (safetyOn){
         if (safetyIn.read() != 1){
             motorsEnable = false;
+            motor1Dir.write(!direction1);
+            motor2Dir.write(!direction2);
         }
     }
 }