Diff: robot.cpp

Revision:

1:a6728adaf7e7

Parent:

0:8a2dd255c508

Child:

2:bf34b86aa0f3

diff -r 8a2dd255c508 -r a6728adaf7e7 robot.cpp
--- a/robot.cpp	Sat Nov 26 17:28:53 2016 +0000
+++ b/robot.cpp	Sat Nov 26 21:43:52 2016 +0000
@@ -5,47 +5,54 @@
 Ticker timer_ticker;
 volatile char i2c_lock = 0;
 
+char debug_mode = DEBUG_MODE;
+char debug_output = DEBUG_OUTPUT_STREAM;
+
 Led led;
 Sensors sensors;
 Motors motors;
 I2C primary_i2c (p9, p10);
 AnalogIn vin_battery(p17);
 Serial pc(USBTX,USBRX);
+Serial bt(p14,p13);
 
 
 // Public functions
 
 void Robot::init(){
-    
     //Start the uptime timer
     timer_minute_count = 0;
     timer_ticker.attach(this,&Robot::_update_minutes, 300);  //Update minutes resets the uptime timer every 5 minutes to prevent stop overruns
     mbed_uptime.start();   
     
+    //Setup serial interfaces
+    setup_serial_interfaces();
+    debug("Practical Robotics Modular Robot\n");
+    
     //Setup the primary i2c bus to 400kHz baud rate
     primary_i2c.frequency(400000);
 
     //Send a reset signal to the LED driver
-    pc.printf("Resetting LED driver    :  ");
-    if(led.reset_led_driver() != 0)pc.printf("FAILED\n");
-    else pc.printf("SUCCESS\n");
+    debug("Resetting LED driver    :  ");
+    if(led.reset_led_driver() != 0)debug("FAILED\n");
+    else debug("SUCCESS\n");
     wait(0.05);
 
     //Send the initialisation signal to the LED driver
-    pc.printf("Initialising LED driver :  ");
-    if(led.init_led_driver() != 0)pc.printf("FAILED\n");
-    else pc.printf("SUCCESS\n");
+    debug("Initialising LED driver :  ");
+    if(led.init_led_driver() != 0)debug("FAILED\n");
+    else debug("SUCCESS\n");
     
     wait(0.05);
 
     //Start the Sharp sensor ticker
-    pc.printf("Starting distance sensor ticker...\n");
+    debug("Starting distance sensor ticker...\n");
     sensors.start_sensor_ticker();
     
     wait(0.05);
     
     //Setup the h-bridge drivers
-    pc.printf("Starting the H-bridge drivers...\n");
+    debug("Starting the H-bridge drivers...\n");
     motors.init();
 }
 
@@ -62,8 +69,44 @@
     return measure_voltage;
 }
 
+void Robot::debug(const char* format, ...)
+{
+    char buffer[256];
+    if (debug_mode) {
+        va_list vl;
+        va_start(vl, format);
+        vsprintf(buffer,format,vl);
+        if(debug_output & 2) bt.printf("%s", buffer);
+        if(debug_output & 1) pc.printf("%s", buffer);
+        va_end(vl);
+    }
+}
+
+void Robot::setup_serial_interfaces()
+{
+    if(ENABLE_PC_SERIAL) {
+        pc.baud(PC_BAUD);
+        pc.attach(this,&Robot::_pc_rx_callback, Serial::RxIrq);
+    }
+    if(ENABLE_BLUETOOTH) {
+        bt.baud(BLUETOOTH_BAUD);
+        bt.attach(this,&Robot::_bt_rx_callback, Serial::RxIrq);
+    }
+}
+
 // Private functions
 
+void Robot::_pc_rx_callback()
+{
+    
+}
+
+void Robot::_bt_rx_callback()
+{
+ 
+}
+
+
 void Robot::_update_minutes()
 {
     mbed_uptime.reset();