From 367b9471d45ae383a531784c1e95de5fe9aa88d9 Mon Sep 17 00:00:00 2001
From: Jianqiao Li <lianqiaolb@gmail.com>
Date: Wed, 10 Feb 2016 18:31:19 -0500
Subject: [PATCH] Fixed cuff problem

---
 common/Motion/motion_physical.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/common/Motion/motion_physical.cpp b/common/Motion/motion_physical.cpp
index f442981..e47b45e 100644
--- a/common/Motion/motion_physical.cpp
+++ b/common/Motion/motion_physical.cpp
@@ -170,8 +170,8 @@ bool MyControllerUpdateData::MyStartup() {
   assemblyState.subscribe(*nh,"robot/state",10);
   jointState.subscribe(*nh,"robot/joint_states",10);
   headState.subscribe(*nh,"robot/head/head_state",10);
-  leftCuffState.state = 0;
-  rightCuffState.state = 0;
+  leftCuffState->state = 0;
+  rightCuffState->state = 0;
   leftCuffState.subscribe(*nh,"robot/digital_io/left_lower_cuff/state",10);
   rightCuffState.subscribe(*nh,"robot/digital_io/right_lower_cuff/state",10);
   leftGripperState.subscribe(*nh,"robot/end_effector/left_gripper/state",10);
@@ -447,7 +447,7 @@ bool MyControllerUpdateData::MyProcessSensors()
 	robotState.rightLimb.sensedEffort[k] = jointState->effort[i];
       }      
     }
-    if(robotState.leftLimb.commandedConfig.empty() || leftCuffState.state==1 /*startup or cuff pressed, reset commanded*/) {
+    if(robotState.leftLimb.commandedConfig.empty() || leftCuffState->state==1 /*startup or cuff pressed, reset commanded*/) {
       SetLimbGovernor(LEFT,LimbState::Normal);
       robotState.leftLimb.controlMode = LimbState::POSITION;
       robotState.leftLimb.sendCommand = false;
@@ -455,7 +455,7 @@ bool MyControllerUpdateData::MyProcessSensors()
       robotState.leftLimb.commandedVelocity = robotState.leftLimb.sensedVelocity;
       robotState.leftLimb.commandedEffort = robotState.leftLimb.sensedEffort;
     }
-    if(robotState.rightLimb.commandedConfig.empty() || rightCuffState.state==1 /*startup or cuff pressed, reset commanded*/) {
+    if(robotState.rightLimb.commandedConfig.empty() || rightCuffState->state==1 /*startup or cuff pressed, reset commanded*/) {
       SetLimbGovernor(RIGHT,LimbState::Normal);
       robotState.rightLimb.controlMode = LimbState::POSITION;
       robotState.rightLimb.sendCommand = false;
@@ -748,6 +748,7 @@ void MyControllerUpdateData::MySendCommands()
 
 void MyControllerUpdateData::MyShutdown()
 {
+  /*
   ros::Publisher enabler = nh->advertise<std_msgs::Bool>("/robot/set_super_enable",10);
   while(ros::ok()) {
     if(!assemblyState->enabled) {
@@ -762,6 +763,7 @@ void MyControllerUpdateData::MyShutdown()
     ros::spinOnce();
     ThreadSleep(0.5);
   }
+  */
 
   ros::shutdown();
 }