Diff: eprom.cpp

Revision:

18:9204f74069b4

Parent:

17:bf614e28668f

Child:

19:3e3b03d80ea3

--- a/eprom.cpp	Sun Jun 04 13:11:09 2017 +0000
+++ b/eprom.cpp	Sun Jun 04 20:22:41 2017 +0000
@@ -111,7 +111,8 @@
     char address_array [2] = {255,0};
     primary_i2c.write(EEPROM_ADDRESS, address_array, 2, false);
     primary_i2c.read(EEPROM_ADDRESS, firmware_bytes, 80, false);
-
+    left_motor_stall_offset = LEFT_STALL * 0.01f;
+    right_motor_stall_offset = RIGHT_STALL * 0.01f;
     if(firmware_bytes[0] == PSI_BYTE) {
         // Parse firmware
         char firmware_string [8];
@@ -149,6 +150,15 @@
             } else motor_calibration_set = 0;
         } else motor_calibration_set = 0;
         if(firmware_version > 1.1) {
+            boot_count = firmware_bytes[69] << 8;
+            boot_count += firmware_bytes[70];
+            boot_count++;
+            eprom.write_firmware_byte(69,(char)(boot_count >> 8));
+            eprom.write_firmware_byte(70,(char)(boot_count % 256));
+            if(motor_calibration_set == 1) {
+                left_motor_stall_offset = (((float) (firmware_bytes[67])) * 0.01f);
+                right_motor_stall_offset = (((float) (firmware_bytes[68])) * 0.01f);
+            }
             base_ir_calibration_set = firmware_bytes[29];
             if(base_ir_calibration_set == 1){
               int white_values[5];
@@ -185,11 +195,11 @@
     write_firmware_byte(29,1);
     for(int i=0;i<5;i++){
        write_firmware_byte(30+i+i,(char) (white_values[i] >> 8));
-       write_firmware_byte(31+i+i,(char) (white_values[i] && 0xFF));
+       write_firmware_byte(31+i+i,(char) (white_values[i] % 256));
     }
      for(int i=0;i<5;i++){
        write_firmware_byte(40+i+i,(char) (black_values[i] >> 8));
-       write_firmware_byte(41+i+i,(char) (black_values[i] && 0xFF));
+       write_firmware_byte(41+i+i,(char) (black_values[i] % 256));
     }
     sensors.IF_set_base_calibration_values(white_values[0], white_values[1], white_values[2], white_values[3], white_values[4], black_values[0], black_values[1], black_values[2], black_values[3], black_values[4]);
 }
@@ -199,15 +209,35 @@
     write_firmware_byte(50,1);
     for(int i=0;i<4;i++){
        write_firmware_byte(51+i+i,(char) (black_values[i] >> 8));
-       write_firmware_byte(52+i+i,(char) (black_values[i] && 0xFF));
+       write_firmware_byte(52+i+i,(char) (black_values[i] % 256));
     }
      for(int i=0;i<4;i++){
        write_firmware_byte(59+i+i,(char) (white_values[i] >> 8));
-       write_firmware_byte(60+i+i,(char) (white_values[i] && 0xFF));
+       write_firmware_byte(60+i+i,(char) (white_values[i] % 256));
     }
     colour.set_calibration_values(black_values[0],black_values[1],black_values[2],black_values[3],white_values[0],white_values[1],white_values[2],white_values[3]);
 }
 
+void Eprom::IF_write_motor_calibration_values(float left_motor_calibration_value, int left_motor_offset, float right_motor_calibration_value, int right_motor_offset){
+    //Set calibration_set byte [22] to 1
+    write_firmware_byte(22,1);
+    int left_motor_cv = left_motor_calibration_value * 16777215;
+    int right_motor_cv = right_motor_calibration_value * 16777215;
+    char lm1 = (char)(left_motor_cv >> 16);
+    char lm2 = (char)(left_motor_cv >> 8 % 256);
+    char lm3 = (char)(left_motor_cv % 256);
+    char rm1 = (char)(right_motor_cv >> 16);
+    char rm2 = (char)(right_motor_cv >> 8 % 256);
+    char rm3 = (char)(right_motor_cv % 256);
+    write_firmware_byte(23,lm1);
+    write_firmware_byte(24,lm2);
+    write_firmware_byte(25,lm3);
+    write_firmware_byte(26,rm1);
+    write_firmware_byte(27,rm2);
+    write_firmware_byte(28,rm3);   
+    write_firmware_byte(67,left_motor_offset);
+    write_firmware_byte(68,right_motor_offset);
+}
 
 void Eprom::update_firmware(){
     psi.debug("\n\nPsiSwarm Firmware Writer\n___________________________________\nUpdating firmware to version %1.1f\n",TARGET_FIRMWARE_VERSION);
@@ -222,7 +252,7 @@
     wait(0.5);
     eprom.write_firmware(firmware_bytes[1], FD_CREATE_DAY, FD_CREATE_MONTH, FD_CREATE_YEAR, firmware_bytes[5], firmware_bytes[6], firmware_bytes[7], firmware_bytes[8],fv_big, 
                          fv_small, has_compass, has_side_ir, has_base_ir, has_base_colour_sensor, has_top_colour_sensor, has_wheel_encoders, has_audio_pic, has_ultrasonic_sensor, has_temperature_sensor, has_recharging_circuit, 
-                         has_433_radio, motor_calibration_set, base_ir_calibration_set, base_colour_calibration_set);
+                         has_433_radio, motor_calibration_set, base_ir_calibration_set, base_colour_calibration_set, boot_count);
 }
 
 
@@ -234,7 +264,7 @@
 
 void Eprom::write_firmware(char _flash_count, char _create_day, char _create_month, char _create_year, char _batch_number, char _serial_number, char _pcb_version_big, char _pcb_version_little, char _firmware_version_big, 
                            char _firmware_version_little, char _has_compass, char _has_side_ir, char _has_base_ir, char _has_base_colour_sensor, char _has_top_colour_sensor, char _has_encoders, char _has_audio_pic, char _has_ultrasonic,
-                           char _has_temperature, char _has_recharging, char _has_433_radio, char _motor_calibration_set, char _base_ir_calibration_set, char _base_colour_calibration_set){
+                           char _has_temperature, char _has_recharging, char _has_433_radio, char _motor_calibration_set, char _base_ir_calibration_set, char _base_colour_calibration_set, short _boot_count){
     psi.debug("Writing new firmware:\n");
     write_firmware_byte(0,PSI_BYTE);
     _flash_count ++;
@@ -295,10 +325,13 @@
     pc.printf("Base colour calibration set: ");
     write_string(_base_colour_calibration_set);
     write_firmware_byte(50,_base_colour_calibration_set);
+    pc.printf("Boot Count: %d\n",_boot_count);
     wait(0.2);
     pc.printf("_________________________________________\n");
     wait(0.2);
     pc.printf("VERIFYING FIRMWARE...\n");
+    short test_b_c = read_firmware_byte(69) << 8;
+    test_b_c += read_firmware_byte(70);
     if(read_firmware_byte(0) == PSI_BYTE
     && read_firmware_byte(1) == _flash_count
     && read_firmware_byte(2) == _create_day
@@ -324,7 +357,7 @@
     && read_firmware_byte(22) == _motor_calibration_set
     && read_firmware_byte(29) == _base_ir_calibration_set
     && read_firmware_byte(50) == _base_colour_calibration_set
-
+    && test_b_c == _boot_count
     ){
         pc.printf("Flash successful.\n");
         display.clear_display();