initializations.cpp

00001  #include <std_msgs/Empty.h>
00002 #include <std_msgs/Int32MultiArray.h>
00003 #include <initializations.h>
00004 #include <ros.h>
00005 #include <definitions.h>
00006 #include <prototypes.h>
00007 #include "mbed.h" 
00008 
00009 // Initializations
00010 
00011 void setupGimbal(){
00012     // Setting Servo Modes
00013     gimbal.setMode(YAWID,SERVO,YAWMIN,YAWMAX);
00014     wait(0.1);
00015     gimbal.setMode(PITCHID,SERVO,PITCHMIN,PITCHMAX);
00016     wait(0.1);
00017     gimbal.setMode(ROLLID,SERVO,ROLLMIN,ROLLMAX);
00018     wait(0.1);   
00019     // Set the Gimbal at the zero position.
00020     gimbal.servo(YAWID, YAWZERO, 50); 
00021     control.yaw = YAWZERO;
00022     control.yawSpeed = 50;
00023     rosInput.yaw = YAWZERO;
00024     rosInput.yawSpeed = 50;
00025     wait(0.1);
00026     gimbal.servo(PITCHID, PITCHZERO, 50);
00027     control.pitch = PITCHZERO;
00028     control.pitchSpeed = 50;
00029     rosInput.pitch = PITCHZERO;
00030     rosInput.pitchSpeed = 50;
00031     wait(0.1);
00032     gimbal.servo(ROLLID, ROLLZERO, 50);
00033     control.roll = ROLLZERO;
00034     control.rollSpeed = 50;
00035     rosInput.roll = ROLLZERO;
00036     rosInput.rollSpeed = 50;
00037 }
00038 
00039 void setupLift(){
00040     
00041     startHallInt();
00042     liftSpeed.period_us(50);
00043     
00044     // Find the bottom position. Go down at lowest speed and set zero point.
00045     liftDirection.write(LIFTDOWN);
00046     liftSpeed.write(0.3);   
00047     
00048     
00049     // Wait until the motor stalls, know the motor is at bottom.
00050     while(stallcount < 3){wait(.01);}
00051     liftSpeed.write(0);
00052     currentPosition = 0;
00053     wait(0.1);
00054     liftDirection.write(LIFTUP);
00055     liftSpeed.write(0.3); 
00056     wait(0.1);
00057     liftSpeed.write(0);
00058     stopHallInt();
00059     rosInput.height = 1;
00060 //    rosInput.height = LIFTHEIGHTMIN;
00061 //    rosInput.heightSpeed = 0;
00062 }
00063 
00064 void setupROSNode(ros::NodeHandle& nh, ros::Subscriber<std_msgs::Int32MultiArray>& sub){     
00065     nh.getHardware()->setBaud(57600);
00066     nh.initNode();
00067     nh.subscribe(sub);
00068 }
00069 
00070 void startHallInt() { // Starting hall effect timer interrupt
00071     hallInt.attach(&hallInterrupt, 0.03);
00072 }
00073 
00074 void stopHallInt() { // Stopping hall effect timer interrupt
00075     hallInt.detach();
00076 }