Revision 4:dc77a25f29de, committed 2016-06-20

Comitter:

jah128

Date:

Mon Jun 20 13:35:06 2016 +0000

Parent:

3:7c0d1f581757

Commit message:

Commit for UKESF Lab

Changed in this revision

--- a/demo.cpp	Tue Mar 15 00:58:09 2016 +0000
+++ b/demo.cpp	Mon Jun 20 13:35:06 2016 +0000
@@ -5,8 +5,9 @@
  * (C) Dept. Electronics & Computer Science, University of York
  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
  *
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
  *
+ * Version 0.5 : Added motor calibration menu
  * Version 0.4 : Added PsiSwarmBasic menu
  * Version 0.2 : Remove most the functionality from center-button push to allow all operations to be accessable from
  *               four directions alone.
@@ -124,7 +125,7 @@
             case 1: //Up pressed
                 switch(level) {
                     case 0:
-                        if(top_menu != 6) {
+                        if(top_menu != 8) {
                             level++;
                             sub_menu = 0;
                         } else {
@@ -286,7 +287,7 @@
             case 2: //Down pressed
                 switch(level) {
                     case 0:
-                        if(top_menu != 6) {
+                        if(top_menu != 8) {
                             level++;
                             sub_menu = 0;
                         } else {
@@ -452,10 +453,12 @@
                 switch(level) {
                     case 0:
                         if(top_menu == 0) {
-                            top_menu = 6;
-                            if(use_flash_basic == 1) top_menu = 7;
+                            top_menu = 8;
                         }
-                        else top_menu --;
+                        else {
+                            if(use_flash_basic == 0 && top_menu == 7) top_menu = 6;
+                            top_menu --;
+                        }
                         break;
                     case 1:
                         switch(top_menu) {
@@ -480,6 +483,10 @@
                                 if(sub_menu == 0) sub_menu = 4;
                                 else sub_menu --;
                                 break;
+                            case 6: //Calibrate Menu
+                                if(sub_menu == 0) sub_menu = 2;
+                                else sub_menu --;
+                                break;
                             case 7: //PsiBasic Menu
                                 if(sub_menu == 0) sub_menu = psi_basic_file_count;
                                 else sub_menu --;
@@ -492,7 +499,7 @@
                 switch(level) {
                     case 0:
                         top_menu ++;
-                        if((top_menu - use_flash_basic) > 6) top_menu = 0;
+                        if((top_menu - use_flash_basic) > 7) top_menu = 0;
                         break;
                     case 1:
                         switch(top_menu) {
@@ -516,6 +523,10 @@
                                 if(sub_menu == 4) sub_menu = 0;
                                 else sub_menu ++;
                                 break;
+                            case 6: //Calibrate Menu
+                                if(sub_menu == 2) sub_menu = 0;
+                                else sub_menu ++;
+                                break;
                             case 7: //PsiBasic Menu
                                 if(sub_menu == psi_basic_file_count) sub_menu = 0;
                                 else sub_menu ++;
@@ -526,7 +537,7 @@
             case 16: //Select pressed
                 switch(level) {
                     case 0:
-                        if(top_menu != 6) {
+                        if(top_menu != 8) {
                             level++;
                             sub_menu = 0;
                         } else {
@@ -584,6 +595,9 @@
                             case 5: // Demo Menu
                                 if(sub_menu == 4) level = 0;
                                 break;
+                            case 6: // Calibrate Menu
+                                if(sub_menu == 2) level = 0;
+                                break;
                             case 7: // PsiBasic Menu
                                 if(sub_menu == psi_basic_file_count) level = 0;
                                 break;
@@ -617,11 +631,14 @@
                     strcpy(topline,"---CODE DEMOS---");
                     break;
                 case 6:
-                    strcpy(topline,"------EXIT------");
+                    strcpy(topline,"-MOTOR CALIBRATE");
                     break;
                 case 7:
                     strcpy(topline,"---PSI BASIC----");
                     break;
+                case 8:
+                    strcpy(topline,"------EXIT------");
+                    break;
             }
             strcpy(bottomline,"");
             break;
@@ -834,7 +851,18 @@
                     }
                     break;
                 case 6:
-                    strcpy(topline,"");
+                    strcpy(topline,"-MOTOR CALIBRATE");
+                    switch(sub_menu) {
+                        case 0:
+                            sprintf(bottomline,"RUN A3 TEST");
+                            break;
+                        case 1:
+                            sprintf(bottomline,"ENTER VALUE");
+                            break;   
+                        case 2:
+                            sprintf(bottomline,"EXIT");
+                            break;
+                    }
                     break;
             }
             break;

--- a/demo.h	Tue Mar 15 00:58:09 2016 +0000
+++ b/demo.h	Mon Jun 20 13:35:06 2016 +0000
@@ -5,9 +5,9 @@
  * (C) Dept. Electronics & Computer Science, University of York
  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
  *
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
  *
- * March 2016
+ * April 2016
  *
  *
  */ 

--- a/eprom.cpp	Tue Mar 15 00:58:09 2016 +0000
+++ b/eprom.cpp	Mon Jun 20 13:35:06 2016 +0000
@@ -5,9 +5,9 @@
  * (C) Dept. Electronics & Computer Science, University of York
  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
  *
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
  *
- * March 2016
+ * April 2016
  *
  * Functions for accessing the 64Kb EPROM chip and reading the reserved firmware block
  *
@@ -105,6 +105,19 @@
         has_temperature_sensor= firmware_bytes[19];
         has_recharging_circuit= firmware_bytes[20];
         has_433_radio= firmware_bytes[21];
+        if(firmware_version > 1.0){
+        motor_calibration_set = firmware_bytes[22];
+        if(motor_calibration_set == 1){
+            left_motor_calibration_value = (float) firmware_bytes[23] * 65536;
+            left_motor_calibration_value += ((float) firmware_bytes[24] * 256);
+            left_motor_calibration_value += firmware_bytes[25];
+            left_motor_calibration_value /= 16777216;
+            right_motor_calibration_value = (float) firmware_bytes[26] * 65536;
+            right_motor_calibration_value += ((float) firmware_bytes[27] * 256);
+            right_motor_calibration_value += firmware_bytes[28];
+            right_motor_calibration_value /= 16777216;
+        }
+        } else motor_calibration_set = 0;
         return 1;
     }
     return 0;

--- a/motors.cpp	Tue Mar 15 00:58:09 2016 +0000
+++ b/motors.cpp	Mon Jun 20 13:35:06 2016 +0000
@@ -5,9 +5,9 @@
  * (C) Dept. Electronics & Computer Science, University of York
  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
  *
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
  *
- * March 2016
+ * April 2016
  *
  *
  */
@@ -17,6 +17,14 @@
 Timeout time_based_action_timeout;
 char brake_when_done = 0;
 
+void set_motor_speed(float left_motor_speed, float right_motor_speed)
+{
+    motor_left_speed = left_motor_speed;
+    motor_right_speed = right_motor_speed;
+    motor_left_brake = 0;
+    motor_right_brake = 0;
+    IF_update_motors();
+}
 void set_left_motor_speed(float speed)
 {
     motor_left_speed = speed;
@@ -229,11 +237,11 @@
     } else {
         if(motor_left_speed >= 0) {
             motor_left_f.write(0);
-            motor_left_r.write(IF_calibrated_speed(motor_left_speed));
+            motor_left_r.write(IF_calibrated_left_speed(motor_left_speed));
 
         } else {
             motor_left_r.write(0);
-            motor_left_f.write(IF_calibrated_speed(-motor_left_speed));
+            motor_left_f.write(IF_calibrated_left_speed(-motor_left_speed));
         }
     }
     if(motor_right_brake) {
@@ -246,16 +254,36 @@
     } else {
         if(motor_right_speed >= 0) {
             motor_right_f.write(0);
-            motor_right_r.write(IF_calibrated_speed(motor_right_speed));
+            motor_right_r.write(IF_calibrated_right_speed(motor_right_speed));
         } else {
             motor_right_r.write(0);
-            motor_right_f.write(IF_calibrated_speed(-motor_right_speed));
+            motor_right_f.write(IF_calibrated_right_speed(-motor_right_speed));
         }
     }
 
 }
 
 
+float IF_calibrated_left_speed(float speed)
+{
+    //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
+    float adjusted  = IF_calibrated_speed(speed);
+    if(use_motor_calibration){
+        adjusted *= left_motor_calibration_value;
+    }
+    return adjusted;
+}
+
+float IF_calibrated_right_speed(float speed)
+{
+    //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
+    float adjusted  = IF_calibrated_speed(speed);
+    if(use_motor_calibration){
+        adjusted *= right_motor_calibration_value;
+    }
+    return adjusted;
+}
+
 float IF_calibrated_speed(float speed)
 {
     if(speed == 0) return 0;

--- a/motors.h	Tue Mar 15 00:58:09 2016 +0000
+++ b/motors.h	Mon Jun 20 13:35:06 2016 +0000
@@ -5,9 +5,9 @@
  * (C) Dept. Electronics & Computer Science, University of York
  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
  *
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
  *
- * March 2016
+ * April 2016
  *
  *
  */ 
@@ -15,6 +15,7 @@
 #ifndef MOTORS_H
 #define MOTORS_H
 
+void set_motor_speed(float left_motor_speed, float right_motor_speed);
 void set_left_motor_speed(float speed);
 void set_right_motor_speed(float speed);
 void brake_left_motor(void);
@@ -35,6 +36,8 @@
 void IF_end_time_based_action();
 
 void IF_update_motors();
+float IF_calibrated_left_speed(float speed);
+float IF_calibrated_right_speed(float speed);
 float IF_calibrated_speed(float speed);
 void IF_init_motors(void);
 

--- 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()

--- a/psiswarm.h	Tue Mar 15 00:58:09 2016 +0000
+++ b/psiswarm.h	Mon Jun 20 13:35:06 2016 +0000
@@ -5,9 +5,9 @@
  * (C) Dept. Electronics & Computer Science, University of York
  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
  *
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
  *
- * March 2016
+ * April 2016
  *
  *
  */ 
@@ -15,7 +15,7 @@
 #ifndef PSISWARM_H
 #define PSISWARM_H
   
-#define SOFTWARE_VERSION_CODE 0.41
+#define SOFTWARE_VERSION_CODE 0.50
 
 #define PIC_ADDRESS 0x30
 #define LCD_ADDRESS 0x7C 
@@ -147,6 +147,11 @@
 extern float center_led_brightness;
 extern float backlight_brightness;
 
+extern char use_motor_calibration;
+extern char motor_calibration_set;
+extern float left_motor_calibration_value;
+extern float right_motor_calibration_value;
+
 extern float firmware_version;
 extern float pcb_version;
 extern float serial_number;

--- a/settings.h	Tue Mar 15 00:58:09 2016 +0000
+++ b/settings.h	Mon Jun 20 13:35:06 2016 +0000
@@ -5,13 +5,18 @@
  * (C) Dept. Electronics & Computer Science, University of York
  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
  *
- * PsiSwarm Library Version: 0.41
+ * PsiSwarm Library Version: 0.5
  *
- * March 2016
+ * April 2016
  *
  *
  */ 
 
+/* USE_MOTOR_CALIBRATION [1=on, recommended      0=off]
+ * If enabled, the actual motor speeds will be adjusted from the requested values using the motor calibration values stored in
+ * firmware (and set using the motor calibration code in the demo mode)
+ */
+#define USE_MOTOR_CALIBRATION 1
 
 /* OFFSET_MOTORS [1=on, recommended      0=off]
  * The motors typically stall when the PWM output is below around 0.2