main.cpp

00001 /*--------------------------------------------------------------------------------
00002 Filename: main.cpp
00003 Description: Main program loop
00004 --------------------------------------------------------------------------------*/
00005 #include "mbed.h"
00006 #include "main.h"
00007 #include <Servo.h>
00008 #include "TOF.h"
00009 #include "Motor.h"
00010 #include "power.h"
00011 #include "Buzzer.h"
00012 #include "LED.h"
00013 #include <ros.h>
00014 #include <std_msgs/UInt8.h>
00015 
00016 
00017 
00018 #include <std_msgs/UInt16.h>
00019 #include <std_msgs/UInt8MultiArray.h>
00020 #include <std_msgs/UInt16MultiArray.h>
00021 #include <std_msgs/UInt32MultiArray.h>
00022 
00023 #include <std_msgs/String.h>
00024 #include <ros/time.h>
00025 #include <sensor_msgs/Range.h>
00026 
00027 
00028 
00029 
00030 class mySTM32 : public MbedHardware
00031 {
00032 public:
00033     mySTM32(): MbedHardware(PD_5, PD_6, 57600) {};
00034 };
00035 
00036 /*
00037 void servo1_cb( const std_msgs::UInt16& cmd_msg) {
00038     servo1.position(cmd_msg.data); //set servo angle, should be from 0-180
00039 }
00040 void servo2_cb( const std_msgs::UInt16& cmd_msg) {
00041     servo2.position(cmd_msg.data); //set servo angle, should be from 0-180
00042 }    
00043 */
00044 
00045 ros::NodeHandle_<mySTM32> nh;
00046 
00047 ros::Subscriber<std_msgs::UInt8> sub("keyControl", &MotorKeySub);  // Subscriber for Motor Control by Keyboard.
00048 ros::Subscriber<std_msgs::UInt8> sub1("ServoControl", &servoKeySub); // Subscriber for Servo Control by keyboard.
00049 
00050 //std_msgs::UInt16MultiArray range_msg;
00051 //ros::Publisher TOFstuff("TOFstuff", &range_msg);
00052 
00053 sensor_msgs::Range range_msg1, range_msg2,range_msg3,range_msg4;
00054 ros::Publisher tof1("tof1", &range_msg1);
00055 ros::Publisher tof2("tof2", &range_msg2);
00056 ros::Publisher tof3("tof3", &range_msg3);
00057 ros::Publisher tof4("tof4", &range_msg4);
00058 
00059 //std_msgs::Int32MultiArray range_msg;
00060 //array.data.clear();
00061 
00062 
00063 /* NOT USED ANYMORE */
00064 //ros::Subscriber<std_msgs::UInt16> sub2("servo1", servo1_cb);
00065 //ros::Subscriber<std_msgs::UInt16> sub3("servo2", servo2_cb);
00066 
00067 
00068 DigitalIn user_button(USER_BUTTON);
00069 float power_levels[3]; //Array to voltage levels
00070 
00071 
00072 Ticker power_monitor;
00073 
00074 //TOF chip shutdown signals
00075 DigitalOut TOF1(PC_8);
00076 DigitalOut TOF4(PC_11);
00077 DigitalOut TOF6(PC_12);
00078 DigitalOut TOF7(PD_2);
00079 
00080 
00081 //Class defines
00082 cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF6,TOF7);  //Define TOF sensor class and initialise devices
00083 cPower cPower(VBATT, V5, V3);
00084 
00085 /*------------------------------------------------------------------------------
00086 Function name: power_check
00087 Input Parameters: N/A
00088 Output Parameters: N/A
00089 Description: Routine interrupt to monitor battery levels
00090 ------------------------------------------------------------------------------*/
00091 
00092 
00093 std_msgs::UInt8MultiArray m;
00094 //sensor_msgs::Range range_msg[4];
00095 
00096  
00097 float TOFRange[4];
00098  
00099 DigitalOut led = LED1;
00100 Timer t;  // Timer 
00101 
00102  char frameid1[] = "/Rear Sensor";
00103  char frameid2[] = "/Front Left Sensor";
00104  char frameid3[] = "/Front Center Sensor"; 
00105  char frameid4[] = "/Front Right Sensor";
00106 /* Private Functions----------------------------------------------------------*/ 
00107 void power_check(void);
00108 
00109 
00110 /*--------------------------------------------------------------------------------
00111                                     MAIN CONTROL LOOP
00112 -------------------------------------------------------------------------------*/
00113 int main()
00114 {
00115     //t.start(); 
00116     IncSize=1;
00117     
00118     nh.initNode();
00119     nh.subscribe(sub);
00120     nh.subscribe(sub1);
00121     //nh.subscribe(sub2);
00122     
00123     nh.advertise(tof1);
00124     nh.advertise(tof2);
00125     nh.advertise(tof3);
00126     nh.advertise(tof4);
00127     
00128     servo1.position(0);
00129     servo1.position(0);
00130     power_monitor.attach(power_check, 30.0); //Monitor battery every 30 seconds
00131     
00132     
00133     //Wait for user button to be pressed
00134     pc.printf("Press user button to start\n\r");
00135     
00136     while(user_button != 1) {}
00137     
00138     while(1) {
00139         
00140         
00141         //Perform TOF measurements 
00142         TOFRange[0] = serviceTOF(VL6180X, ADDR1);
00143         TOFRange[1] = serviceTOF(VL6180X, ADDR4);
00144         TOFRange[2] = serviceTOF(VL6180X, ADDR6);
00145         TOFRange[3] = serviceTOF(VL6180X, ADDR7);
00146 
00147         //Check_for_obstacles(TOFRange); //Run obstacle avoidance
00148        range_msg1.header.frame_id =frameid1;
00149        range_msg2.header.frame_id =frameid2;
00150        range_msg3.header.frame_id =frameid3;
00151        range_msg4.header.frame_id =frameid4;
00152        
00153        range_msg1.range = TOFRange[0];
00154        range_msg2.range = TOFRange[1];
00155        range_msg3.range = TOFRange[2];
00156        range_msg4.range = TOFRange[3];
00157         tof1.publish(&range_msg1);
00158         tof2.publish(&range_msg2);
00159         tof3.publish(&range_msg3);
00160         tof4.publish(&range_msg4);
00161         
00162         // Need to write code like the obstical ovaoidance, to stop the motors, by the nucleo 
00163         // sending a char to the PI
00164         
00165         //pc.printf("Spinning...");
00166         nh.spinOnce();
00167         wait_ms(5);
00168     }
00169 }
00170 
00171 void power_check()
00172 {
00173     power_levels[0] = cPower.monitor_battery(); //Monitors raw battery
00174     power_levels[1] = cPower.monitor_5v_line(); //Monitors 5V line
00175     power_levels[2] = cPower.monitor_3v3_line();//Monitors 3V3 Line
00176 
00177     update_power_levels(power_levels[0]); //Sends the raw battery levels to the LED class 
00178 }