Diff: actuators.cpp

Revision:

93:a8898eb80edc

Parent:

91:6bbbbc6643c8

Child:

94:28e274481b60

Child:

97:0f67952051e5

--- a/actuators.cpp	Wed Oct 21 10:13:10 2015 +0000
+++ b/actuators.cpp	Wed Oct 21 13:16:24 2015 +0200
@@ -5,6 +5,7 @@
 #include "encoder.h"
 #include "HIDScope.h"
 #include "Servo.h"
+#include "buttons.h"
     // functions for controlling the motors
     bool motorsEnable = false;
     bool safetyOn = false; // start with safety off for calibration
@@ -46,6 +47,15 @@
     double timechange;
     bool pidOut = 0;
 
+    // for Calibration:
+    bool calibrating1 = true;
+    bool calibrating2 = false;
+    double motorCalSpeed = 10; // deg/sec
+    double returnSpeed = -10;
+    bool springHit = false;
+    float lastCall = 0;
+
+
     // Create object instances
     // Safety Pin
     DigitalIn safetyIn(safetyPin);
@@ -167,32 +177,47 @@
 }
 
 void calibrateMotors(){
-//    safetyOn = false;
-//    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.setPosition(0); // set motor 1 cal angle
-//                // move away
-//            }
-//        }
-//        if (calibrating1){
-//            motor2SetSpeed = motorCalSpeed; // deg/sec 
-//            if(safetyIn.read() !=1){ // check if arm reached safety position
-//                calibrating2 = false; // done 
-//                motor2SetSpeed = 0; // brake motor
-//                encoder2.setPosition(0); // set motor 2 cal angle
-//                // move away
-//            }
-//        }
-//        writeMotors();
-//        wait(0.01f);
-//    }
+   safetyOn = false;
+   redLed.write(0); greenLed.write(1); blueLed.write(1);
+   while (calibrating1 || calibrating2){
+       if (calibrating1){
+            motor1SetSpeed = motorCalSpeed; // deg/sec 
+            redLed.write(1); greenLed.write(0); blueLed.write(1);
+           if(safetyIn.read() !=1){ // check if arm reached safety position
+               encoder1.setPosition(0); // set motor 1 cal angle
+               motor1SetSpeed = returnSpeed; // move away
+               springHit = true;
+           }else{ 
+               if(springHit){ // if hit and after is no longer touching spring
+                motor1SetSpeed = 0;
+                springHit = false;
+                calibrating1 = false;
+                calibrating2 = true; // start calibrating 2
+               }
+           }
+       }
+       if (calibrating2){
+            motor2SetSpeed = motorCalSpeed; 
+            redLed.write(1); greenLed.write(1); blueLed.write(0);
+           if(safetyIn.read() !=1){ // check if arm reached safety position
+               encoder2.setPosition(0); // set motor 2 cal angle
+               motor2SetSpeed = returnSpeed; // move away
+               springHit = true;
+           }else{ 
+               if(springHit){ // if hit and after is no longer touching spring
+                motor2SetSpeed = 0;
+                springHit = false;
+                calibrating2 = false; // stop calibrating 2
+               }
+           }
+       }
+       now = t.read(); // call motor using timer instead of wait
+       if(now - lastCall > motorCall){
+           writeMotors();
+           lastCall = now;
+       }
+
+   }
     safetyOn = true; // turn safety on after callibration
 }