Diff: actuators.cpp

Revision:

85:03dc24043236

Parent:

84:df770ec4df61

Child:

86:a5f48ae7096e

--- a/actuators.cpp	Tue Oct 20 15:07:51 2015 +0200
+++ b/actuators.cpp	Tue Oct 20 15:28:11 2015 +0200
@@ -133,7 +133,6 @@
     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); // change pid output direction
@@ -141,7 +140,6 @@
         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);
@@ -169,36 +167,28 @@
 }
 
 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;
+    double motorCalSpeed = 10; // deg/sec
+    while (calibrating1 || calibrating2){
+        if (calibrating1){
+            motor1SetSpeed = motorCalSpeed; // deg/sec 
+            if(safetyIn.read() !=1){ // check if arm reached safety position
+                calibrating1 = false; // done 
+                motor1SetSpeed = 0; // brake motor
+                encoder1.setValue(0); // set motor 1 cal angle
+            }
+        }
+        if (calibrating1){
+            motor2SetSpeed = motorCalSpeed; // deg/sec 
+            if(safetyIn.read() !=1){ // check if arm reached safety position
+                calibrating2 = false; // done 
+                motor2SetSpeed = 0; // brake motor
+                encoder2.setValue(0); // set motor 2 cal angle
+            }
+        }
+        wait(0.2f);
+    }
     safetyOn = true; // turn safety on after callibration
 }
 
@@ -207,8 +197,6 @@
     if (safetyOn){
         if (safetyIn.read() != 1){
             motorsEnable = false;
-            motor1Dir.write(!direction1);
-            motor2Dir.write(!direction2);
         }
     }
 }