Diff: psiswarm.cpp

Revision:

4:dc77a25f29de

Parent:

2:c6986ee3c7c5

--- a/psiswarm.cpp	Tue Mar 15 00:58:09 2016 +0000
+++ b/psiswarm.cpp	Mon Jun 20 13:35:06 2016 +0000
@@ -82,6 +82,12 @@
 char switch_byte;
 char previous_switch_byte;
 
+
+char use_motor_calibration = USE_MOTOR_CALIBRATION;
+char motor_calibration_set;
+float left_motor_calibration_value = 1.0;
+float right_motor_calibration_value = 1.0;
+
 char debug_mode = DEBUG_MODE;
 char debug_output = DEBUG_OUTPUT_STREAM;
 
@@ -153,6 +159,23 @@
     if(read_firmware() == 1) {
         debug("Version %3.2f\n",firmware_version);
         IF_get_hardware_description();
+        if(use_motor_calibration){
+            if(!motor_calibration_set){
+                if(firmware_version < 1.1){
+                    debug("- WARNING: This firmware is incompatible with motor calibration");
+                    debug("- WARNING: Please update the firmware to use this feature.");
+                    use_motor_calibration = 0;   
+                }   
+                else {
+                    debug("- WARNING: Motor calibration values have not been stored in firmware");
+                    debug("- WARNING: Run calibration routine to use this feature.");
+                    use_motor_calibration = 0;   
+                }
+            }
+            else {
+                debug("- Motor calibration in use [LEFT:%0.4f RIGHT:%0.4f]",left_motor_calibration_value,right_motor_calibration_value);   
+            }
+        }
     } else {
         debug("INVALID\n");
         debug("- WARNING: Check firmware to enable robot features");
@@ -209,6 +232,13 @@
         if(demo > 0) demo_mode();
         display.init_display(0);
     }
+    wait(1.0);
+    display.clear_display();
+    display.set_position(0,0);
+    display.write_string(program_name);
+    display.set_position(1,0);
+    display.write_string(author_name);
+    wait(0.1);
 }
 
 void IF_update_minutes()