Diff: psiswarm.cpp

Revision:

1:060690a934a9

Parent:

0:d6269d17c8cf

Child:

2:c6986ee3c7c5

--- a/psiswarm.cpp	Thu Feb 04 21:48:54 2016 +0000
+++ b/psiswarm.cpp	Thu Mar 03 23:21:47 2016 +0000
@@ -1,5 +1,5 @@
 /* University of York Robotics Laboratory PsiSwarm Library: PsiSwarm Core Source File
- * 
+ *
  * File: psiswarm.cpp
  *
  * (C) Dept. Electronics & Computer Science, University of York
@@ -10,7 +10,7 @@
  * February 2016
  *
  *
- */ 
+ */
 
 #include "psiswarm.h"
 
@@ -47,6 +47,21 @@
 Ticker timer_ticker;
 
 float firmware_version;
+float pcb_version;
+float serial_number;
+
+char has_compass=0;
+char has_side_ir=1;
+char has_base_ir=1;
+char has_base_colour_sensor=0;
+char has_top_colour_sensor=0;
+char has_wheel_encoders=0;
+char has_audio_pic=0;
+char has_ultrasonic_sensor=0;
+char has_temperature_sensor=0;
+char has_recharging_circuit=0;
+char has_433_radio=0;
+
 char robot_id;
 char previous_robot_id;
 
@@ -69,7 +84,7 @@
 
 char debug_mode = DEBUG_MODE;
 char debug_output = DEBUG_OUTPUT_STREAM;
- 
+
 char firmware_bytes[21];
 
 int base_colour_sensor_raw_values [4];
@@ -115,6 +130,8 @@
 int base_ir_pulse_delay = DEFAULT_BASE_IR_PULSE_DELAY;
 
 
+
+
 /**
  * init()
  *
@@ -133,57 +150,69 @@
     debug("PsiSwarm Robot Library %1.2f\n\n",SOFTWARE_VERSION_CODE);
     debug("- Setting up serial interface\n");
     debug("- Reading firmware: ");
-    if(read_firmware() == 1){
-        debug("Version %3.2f\n",firmware_version);   
-    }else debug("INVALID\n");
-    
-    if(ENABLE_BASIC == 1){
-        read_list_of_file_names();   
-        if(psi_basic_file_count == 0){
-            debug("- No PsiBasic files found\n");   
-        }
-        else use_flash_basic = 1;
+    if(read_firmware() == 1) {
+        debug("Version %3.2f\n",firmware_version);
+        IF_get_hardware_description();
+    } else {
+        debug("INVALID\n");
+        debug("- WARNING: Check firmware to enable robot features");
+    }
+    if(ENABLE_BASIC == 1) {
+        read_list_of_file_names();
+        if(psi_basic_file_count == 0) {
+            debug("- No PsiBasic files found\n");
+        } else use_flash_basic = 1;
     }
     debug("- Setting up PIC microcontroller\n");
     // IF_check_pic_firmware();
     debug("- Setting up LED drivers\n");
-    IF_init_leds();    
+    IF_init_leds();
     if(IF_setup_led_expansion_ic() != 0) {
-        debug("- WARNING: No I2C acknowledge for LED driver\n");   
-        system_warnings += 1; 
+        debug("- WARNING: No I2C acknowledge for LED driver\n");
+        system_warnings += 1;
     }
     debug("- Setting up motor drivers\n");
-    IF_init_motors();   
+    IF_init_motors();
     debug("- Setting up GPIO expansion\n");
     reset_encoders();
     IF_setup_gpio_expansion_ic();
-    debug("- Setting up temperature sensor\n");
-    IF_setup_temperature_sensor();
-    debug("- Setting up base colour sensor\n");
-    IF_check_base_colour_sensor();
-    debug("- Setting up ultrasonic sensor\n");
-    //enable_ultrasonic_ticker();
+    if(has_temperature_sensor) {
+        debug("- Setting up temperature sensor\n");
+        IF_setup_temperature_sensor();
+    }
+    if(has_base_colour_sensor) {
+        debug("- Setting up base colour sensor\n");
+        IF_check_base_colour_sensor();
+    }
+    if(has_ultrasonic_sensor) {
+        debug("- Setting up ultrasonic sensor\n");
+        //enable_ultrasonic_ticker();
+    }
+
     debug("- Robot ID: %d\n",robot_id);
-    char switchstate = IF_get_switch_state(); 
+    char switchstate = IF_get_switch_state();
     debug("- Switch State   : %d\n",switchstate);
     debug("- Battery Voltage: %1.3fV\n",get_battery_voltage());
     debug("- DC Voltage     : %1.3fV\n",get_dc_voltage());
     debug("- Current Draw   : %1.3fA\n",get_current());
+    if(has_temperature_sensor){
     debug("- Temperature    : %1.3fC\n",get_temperature());
+    }
     char demo = 0;
     if(ENABLE_DEMO == 1 && switchstate > 0) demo=1;
     display.init_display(demo);
     event_handler.attach_us(&IF_handle_events, 1000);
-    if(demo > 0){      
+    if(demo > 0) {
         debug("- Demo mode button is pressed\n");
         wait(1.0);
         demo = IF_get_switch_state();
         if(demo > 0) demo_mode();
-        display.init_display(0);  
+        display.init_display(0);
     }
 }
 
-void IF_update_minutes(){
+void IF_update_minutes()
+{
     uptime.reset();
     timer_minute_count += 5;
 }
@@ -192,27 +221,27 @@
 {
     // This is the main 'operating system' thread that handles events from robot stimuli
     // By default it is run every 1ms and checks if there are events to handle
-    if(event > 0){
+    if(event > 0) {
         // There are some events to handle.  We don't handle all events in every loop to keep the system responsive, instead they are priorised.
-        if(encoder_event == 1){
+        if(encoder_event == 1) {
             // The encoders have changed; update the encoder values
             IF_update_encoders();
-            encoder_event = 0; 
-            event--;  
+            encoder_event = 0;
+            event--;
         }  else {
-            if(switch_event == 1){
-               IF_update_switch();
-               switch_event = 0;
-               event--; 
+            if(switch_event == 1) {
+                IF_update_switch();
+                switch_event = 0;
+                event--;
             }
-            if(change_id_event == 1){
+            if(change_id_event == 1) {
                 // The user ID for the robot has been changed
                 IF_update_user_id();
                 change_id_event = 0;
                 event--;
             }
         }
-    }   
+    }
 }
 
 void IF_update_encoders()
@@ -246,34 +275,55 @@
 void reset_encoders()
 {
     left_encoder = 0;
-    right_encoder = 0;    
+    right_encoder = 0;
 }
 
-void debug(const char* format, ...) 
+void 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);
-  if(debug_output & 4) display.debug_page(buffer,strlen(buffer));
-  va_end(vl);
-  }
+    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);
+        if(debug_output & 4) display.debug_page(buffer,strlen(buffer));
+        va_end(vl);
+    }
 }
 
 float get_uptime(void)
 {
-    return uptime.read() + (timer_minute_count * 60);   
+    return uptime.read() + (timer_minute_count * 60);
 }
 
-void pause_user_code(float period){
+void pause_user_code(float period)
+{
     user_code_restore_mode = user_code_running;
     user_code_running = 0;
     pause_usercode_timeout.attach(&IF_end_pause_user_code, period);
 }
 
-void IF_end_pause_user_code(){
-     user_code_running = user_code_restore_mode;
+void IF_end_pause_user_code()
+{
+    user_code_running = user_code_restore_mode;
+}
+
+void IF_get_hardware_description()
+{
+        debug("- Robot serial number %1.2f\n",serial_number);
+        debug("- PCB version %1.2f\n",pcb_version);
+        debug("- Hardware: ");
+        if(has_compass) debug("COMPASS, ");
+        if(has_side_ir) debug("SIDE IR, ");
+        if(has_base_ir) debug("BASE IR, ");
+        if(has_base_colour_sensor) debug("BASE COLOUR, ");
+        if(has_top_colour_sensor) debug("TOP COLOUR, ");
+        if(has_wheel_encoders) debug("WHEEL ENC., ");
+        if(has_audio_pic) debug("AUDIO, ");
+        if(has_ultrasonic_sensor) debug("ULTRASONIC, ");
+        if(has_temperature_sensor) debug("TEMPERATURE, ");
+        if(has_recharging_circuit) debug("RECHARGING, ");
+        if(has_433_radio) debug("433 RADIO.");
+        debug("\n");
 }
\ No newline at end of file