Add Enable Lock

221robotics · Sep 15, 2013 · 62cdb80 · 62cdb80
1 parent 5aabea9
commit 62cdb80
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 2 deletions.
diff --git a/libraries/RobotOpen/RobotOpen.cpp b/libraries/RobotOpen/RobotOpen.cpp
@@ -53,6 +53,7 @@ static unsigned int _outgoingPacketSize = 1;
 
 // Robot specific stuff
 static boolean _enabled = false;            // Tells us if the robot is enabled or disabled
+static boolean _enable_lock = false;
 static unsigned long _lastPacket = 0;       // Keeps track of the last time (ms) we received data
 static unsigned long _lastTimedLoop = 0;    // Keeps track of the last time the timed loop ran
 static unsigned long _lastDSLoop = 0;       // Keeps track of the last time we published DS data
@@ -204,7 +205,7 @@ void RobotOpenClass::syncDS() {
     wdt_reset();
 
     // detect disconnect
-    if ((millis() - _lastPacket) > 200) {  // Disable the robot, drop the connection
+    if ((millis() - _lastPacket) > 200 && !_enable_lock) {  // Disable the robot, drop the connection
         _enabled = false;
         // NO CONNECTION -- blue LED
         digitalWrite(4, LOW);
@@ -215,7 +216,7 @@ void RobotOpenClass::syncDS() {
             firstEnableLoop = true;
         }
 	}
-    else if (_enabled == true) {
+    else if (_enabled == true || _enable_lock) {
         // ENABLED -- green LED
         digitalWrite(4, LOW);
         digitalWrite(5, HIGH);
@@ -418,6 +419,16 @@ void RobotOpenClass::parsePacket() {
               _lastPacket = millis();
               break;
 
+            case 'l': // enable lock
+              _enable_lock = true;
+              _lastPacket = millis();
+              break;
+
+            case 'u': // enable unlock
+              _enable_lock = false;
+              _lastPacket = millis();
+              break;
+
             case 'c': // control packet
               _enabled = true;
               _lastPacket = millis();
@@ -590,6 +601,10 @@ boolean RobotOpenClass::enabled() {
     return _enabled;
 }
 
+void RobotOpenClass::enableLock(boolean lock) {
+    _enable_lock = lock;
+}
+
 int RobotOpenClass::numJoysticks() {
     return _total_joysticks;
 }
diff --git a/libraries/RobotOpen/RobotOpen.h b/libraries/RobotOpen/RobotOpen.h
@@ -43,6 +43,9 @@ class RobotOpenClass {
     // Tells us if the robot is enabled
     static boolean enabled();
 
+    // Sets if robot will start enabled or rely on DS
+    static void enableLock(boolean lock);
+
     // How many joysticks are being received
     static int numJoysticks();