main.cpp

00001  /*
00002  * Code for the Module Microcontroller as part of the 
00003  * Autonomous Camera Kart project.
00004  * Controls the Gimbal motors, raise and lower mechanism,
00005  * IMU sensor input, communication with the NUC onboard computer.
00006  */
00007 
00008 // Library Includes
00009 #include <stdint.h>
00010 #include "mbed.h"
00011 #include <ros.h>
00012 #include <std_msgs/Empty.h>
00013 #include <std_msgs/Int32MultiArray.h>
00014 #include <BNO055.h>
00015 #include <initializations.h>
00016 #include <definitions.h>
00017 #include <prototypes.h>
00018 #include <Mx28.h>
00019 
00020 // Micro Module Declarations
00021 PwmOut              liftSpeed(D10); // Normally D10
00022 DigitalOut          liftDirection(D11);
00023 DigitalIn           button(USER_BUTTON);
00024 Ticker              hallInt;
00025 Timer               filter_hall1;
00026 Timer               rosMode_Delay;
00027 DynamixelClass      gimbal(57600, D7, D8, D2);
00028 InterruptIn         eStop(D5);
00029 
00030 ros::NodeHandle     nh;
00031 ros::Subscriber<std_msgs::Int32MultiArray> sub("module_command", &module_commandCB);
00032 
00033 // Global Variable
00034 struct          fields control;
00035 struct          fields rosInput;
00036 float           liftDutyCycle = 0;
00037 volatile int    currentPosition = 0;
00038 int             prevPosition = 0;
00039 int             stallcount = 0;
00040 bool            rosFlag = 1;
00041 bool            eStopFlag = 0;
00042 
00043 // Main Program
00044 int main() {
00045     // Initializations
00046     filter_hall1.start();                       //Start Filtering Timer
00047     eStop.fall(&eStopOn);
00048     eStop.rise(&eStopOff);   
00049     setupGimbal();                              // Gimbal
00050     setupLift();                                // Lift
00051     setupROSNode(nh, sub);                     // ROS Node Handle
00052     rosInput.mode = 0;
00053 
00054     // Main Loop
00055     while (1) { 
00056     //ROS Functions 
00057         nh.spinOnce();
00058         rosCheck();    
00059     // Lift Operation
00060         runLift();
00061     //Run Gimbal 
00062         runGimbal();
00063     }     
00064 }