UVic Assistive Technology Lab / Mbed 2 deprecated DSLR_Camera_Gimbal

Dependencies:   mbed ros_lib_kinetic

Revision:
7:950b3c3b5a2b
Parent:
6:2ffa254e8f6e
Child:
10:836e701d00a6
--- a/initializations.cpp	Tue Mar 06 21:44:07 2018 +0000
+++ b/initializations.cpp	Wed Mar 14 19:33:08 2018 +0000
@@ -1,5 +1,5 @@
  #include <std_msgs/Empty.h>
-#include <std_msgs/Float32MultiArray.h>
+#include <std_msgs/Int32MultiArray.h>
 #include <initializations.h>
 #include <ros.h>
 #include <definitions.h>
@@ -34,17 +34,9 @@
     liftSpeed.write(0);
     currentPosition = 0;
     stopHallInt();
-    wait(0.1);
-    // Lift up from stalling position
-    liftDirection.write(LIFTUP);
-    liftSpeed.write(0.3);
-    wait(0.1);
-    //Stop
-    liftSpeed.write(0);
 }
 
-void setupROSNode(ros::NodeHandle& nh, ros::Subscriber<std_msgs::Float32MultiArray>& sub){
-       
+void setupROSNode(ros::NodeHandle& nh, ros::Subscriber<std_msgs::Int32MultiArray>& sub){     
     nh.getHardware()->setBaud(57600);
     nh.initNode();
     nh.subscribe(sub);
@@ -57,4 +49,3 @@
 void stopHallInt() { // Stopping hall effect timer interrupt
     hallInt.detach();
 }
-