Diff: initializations.cpp

Revision:

1:1ac7d472cfa2

Parent:

0:3a767f41cf04

Child:

2:0537a8007a39

diff -r 3a767f41cf04 -r 1ac7d472cfa2 initializations.cpp
--- a/initializations.cpp	Wed Jan 31 05:24:12 2018 +0000
+++ b/initializations.cpp	Tue Feb 06 20:50:55 2018 +0000
@@ -20,15 +20,13 @@
     gimbal.servo(ROLLID, ROLLZERO, 50);
     control.roll = ROLLZERO;
     dynaInt.attach(&motorInterrupt, 0.0001); // Update Dynamixel at 1kHz.
-    
     rosInt.attach(&rosInterrupt, 0.1);
 }
 
 void setupLift(){
     
-    int position = 0;
-    
-    hallInt.attach(&hallInterrupt, 0.0001);
+    hallInt.attach(&hallInterrupt, 0.05);
+    liftInt.attach(&liftInterrupt, 0.1);
     liftSpeed.period_us(50);
     
     // Find the bottom position. Go down at lowest speed and set zero point.
@@ -36,27 +34,12 @@
     liftSpeed.write(0.3);   
     
     // Wait until the motor stalls, know the motor is at bottom.
-    while(!stall){
-        if (liftFlag){
-            liftFlag = 0;
-            checkLift(position, stall);
-        }
-    }
-
-    // Go up 0.3 cm from the bottom.
-    liftDirection.write(LIFTUP);
-    liftSpeed.write(0.3);
-    position = 0;
-    stall = FALSE;
-    while(position <  LIFTHEIGHTMIN){
-        if (liftFlag){
-            liftFlag = 0;
-            checkLift(position, stall);
-        }
-    }
-    liftSpeed.write(0);
-    control.height = currentPosition;
-    control.liftRun = FALSE;
+    while(!stall){}
+    if(stall == true){
+       liftSpeed.write(0);
+       currentPosition = 0;
+       stall = false; 
+    } 
 }
 
 void setupROSNode(ros::NodeHandle& nh, ros::Subscriber<std_msgs::Float32MultiArray>& sub){