Psi Swarm Robot
Psi Swarm robot library version 0.9
Fork of
PsiSwarmV9
by 
James Hilder
Diff: eprom.cpp
- Revision:
- 17:bf614e28668f
- Parent:
- 14:2f1ad77d281e
- Child:
- 18:9204f74069b4
diff -r 50686c07ad07 -r bf614e28668f eprom.cpp
--- a/eprom.cpp Thu Jun 01 23:02:32 2017 +0000
+++ b/eprom.cpp Sun Jun 04 13:11:09 2017 +0000
@@ -81,6 +81,27 @@
return data [0];
}
+void Eprom::write_firmware_byte ( int address, char data ){
+ char write_array[3];
+ write_array[0] = 255;
+ write_array[1] = address % 256;
+ write_array[2] = data;
+ primary_i2c.write(EEPROM_ADDRESS, write_array, 3, false);
+ //Takes 5ms to write a page: ideally this could be done with a timer or RTOS
+ wait(0.005);
+}
+
+
+char Eprom::read_firmware_byte ( int address ){
+ char address_array [2];
+ address_array[0] = 255;
+ address_array[1] = address % 256;
+ char data [1];
+ primary_i2c.write(EEPROM_ADDRESS, address_array, 2, false);
+ primary_i2c.read(EEPROM_ADDRESS, data, 1, false);
+ return data [0];
+}
+
/** Read the data stored in the reserved firmware area of the EPROM
*
* @return 1 if a valid firmware is read, 0 otherwise
@@ -89,7 +110,7 @@
{
char address_array [2] = {255,0};
primary_i2c.write(EEPROM_ADDRESS, address_array, 2, false);
- primary_i2c.read(EEPROM_ADDRESS, firmware_bytes, 21, false);
+ primary_i2c.read(EEPROM_ADDRESS, firmware_bytes, 80, false);
if(firmware_bytes[0] == PSI_BYTE) {
// Parse firmware
@@ -100,7 +121,8 @@
sprintf(pcb_version_string,"%d.%d",firmware_bytes[7],firmware_bytes[8]);
pcb_version = atof(pcb_version_string);
char serial_number_string [8];
- sprintf(serial_number_string,"%d.%d",firmware_bytes[5],firmware_bytes[6]);
+ if(firmware_bytes[6] > 9) sprintf(serial_number_string,"%d.%d",firmware_bytes[5],firmware_bytes[6]);
+ else sprintf(serial_number_string,"%d.0%d",firmware_bytes[5],firmware_bytes[6]);
serial_number = atof(serial_number_string);
has_compass = firmware_bytes[11];
has_side_ir = firmware_bytes[12];
@@ -124,9 +146,222 @@
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;
} else motor_calibration_set = 0;
+ if(firmware_version > 1.1) {
+ base_ir_calibration_set = firmware_bytes[29];
+ if(base_ir_calibration_set == 1){
+ int white_values[5];
+ int black_values[5];
+ for(int i=0;i<5;i++){
+ int k_val = i+i;
+ white_values[i] = (firmware_bytes[30 + k_val] << 8) + firmware_bytes[31 + k_val];
+ black_values[i] = (firmware_bytes[40 + k_val] << 8) + firmware_bytes[41 + k_val];
+ }
+ 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]);
+ }else base_ir_calibration_set = 0;
+ base_colour_calibration_set = firmware_bytes[50];
+ if(base_colour_calibration_set == 1){
+ int white_values[4];
+ int black_values[4];
+ for(int i=0;i<4;i++){
+ int k_val = i+i;
+ white_values[i] = (firmware_bytes[51 + k_val] << 8) + firmware_bytes[52 + k_val];
+ black_values[i] = (firmware_bytes[59 + k_val] << 8) + firmware_bytes[60 + k_val];
+ }
+ 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]);
+ }else base_colour_calibration_set = 0;
+ } else {
+ base_ir_calibration_set = 0;
+ base_colour_calibration_set = 0;
+ }
return 1;
}
return 0;
-}
\ No newline at end of file
+}
+
+void Eprom::IF_write_base_ir_calibration_values(short white_values[5], short black_values[5]){
+ //Set calibration_set byte [29] to 1
+ 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));
+ }
+ 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));
+ }
+ 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]);
+}
+
+void Eprom::IF_write_base_colour_calibration_values(int white_values[4], int black_values[4]){
+ //Set calibration_set byte [50] to 1
+ 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));
+ }
+ 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));
+ }
+ 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::update_firmware(){
+ psi.debug("\n\nPsiSwarm Firmware Writer\n___________________________________\nUpdating firmware to version %1.1f\n",TARGET_FIRMWARE_VERSION);
+ display.clear_display();
+ display.set_position(0,0);
+ display.write_string("UPDATING");
+ display.set_position(1,0);
+ display.write_string("FIRMWARE");
+ char fv_big = (char) TARGET_FIRMWARE_VERSION;
+ char fv_small = (char) ((float) (TARGET_FIRMWARE_VERSION - fv_big) * 10.0f);
+
+ 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);
+}
+
+
+
+void Eprom::write_string(char value){
+ if(value==1) pc.printf("YES\n");
+ else pc.printf("NO\n");
+}
+
+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){
+ psi.debug("Writing new firmware:\n");
+ write_firmware_byte(0,PSI_BYTE);
+ _flash_count ++;
+ pc.printf("Flash Count: %d\n",_flash_count);
+ write_firmware_byte(1,_flash_count);
+ pc.printf("Flash Date: %d-%d-%d\n",_create_day,_create_month,_create_year);
+ write_firmware_byte(2,_create_day);
+ write_firmware_byte(3,_create_month);
+ write_firmware_byte(4,_create_year);
+ pc.printf("Serial Number: %d-%d\n",_batch_number,_serial_number);
+ write_firmware_byte(5,_batch_number);
+ write_firmware_byte(6,_serial_number);
+ pc.printf("PCB Version: %d.%d\n",_pcb_version_big,_pcb_version_little);
+ write_firmware_byte(7,_pcb_version_big);
+ write_firmware_byte(8,_pcb_version_little);
+ pc.printf("Firmware Version: %d.%d\n",_firmware_version_big,_firmware_version_little);
+ write_firmware_byte(9,_firmware_version_big);
+ write_firmware_byte(10,_firmware_version_little);
+ pc.printf("Has Compass: ");
+ write_string(_has_compass);
+ write_firmware_byte(11,_has_compass);
+ pc.printf("Has Side IR Sensors: ");
+ write_string(_has_side_ir);
+ write_firmware_byte(12,_has_side_ir);
+ pc.printf("Has Base IR Sensors: ");
+ write_string(_has_base_ir);
+ write_firmware_byte(13,_has_base_ir);
+ pc.printf("Has Base Colour Sensor: ");
+ write_string(_has_base_colour_sensor);
+ write_firmware_byte(14,_has_base_colour_sensor);
+ pc.printf("Has Top Colour Sensor: ");
+ write_string(_has_top_colour_sensor);
+ write_firmware_byte(15,_has_top_colour_sensor);
+ pc.printf("Has Wheel Encoders: ");
+ write_string(_has_encoders);
+ write_firmware_byte(16,_has_encoders);
+ pc.printf("Has Audio PIC: ");
+ write_string(_has_audio_pic);
+ write_firmware_byte(17,_has_audio_pic);
+ pc.printf("Has Ultrasonic Sensor: ");
+ write_string(_has_ultrasonic);
+ write_firmware_byte(18,_has_ultrasonic);
+ pc.printf("Has Temperature Sensor: ");
+ write_string(_has_temperature);
+ write_firmware_byte(19,_has_temperature);
+ pc.printf("Has Recharging Circuit: ");
+ write_string(_has_recharging);
+ write_firmware_byte(20,_has_recharging);
+ pc.printf("Has 433MHz Radio: ");
+ write_string(_has_433_radio);
+ write_firmware_byte(21,_has_433_radio);
+ pc.printf("Motor calibration set: ");
+ write_string(_motor_calibration_set);
+ write_firmware_byte(22,_motor_calibration_set);
+ pc.printf("Base IR calibration set: ");
+ write_string(_base_ir_calibration_set);
+ write_firmware_byte(29,_base_ir_calibration_set);
+ pc.printf("Base colour calibration set: ");
+ write_string(_base_colour_calibration_set);
+ write_firmware_byte(50,_base_colour_calibration_set);
+ wait(0.2);
+ pc.printf("_________________________________________\n");
+ wait(0.2);
+ pc.printf("VERIFYING FIRMWARE...\n");
+ if(read_firmware_byte(0) == PSI_BYTE
+ && read_firmware_byte(1) == _flash_count
+ && read_firmware_byte(2) == _create_day
+ && read_firmware_byte(3) == _create_month
+ && read_firmware_byte(4) == _create_year
+ && read_firmware_byte(5) == _batch_number
+ && read_firmware_byte(6) == _serial_number
+ && read_firmware_byte(7) == _pcb_version_big
+ && read_firmware_byte(8) == _pcb_version_little
+ && read_firmware_byte(9) == _firmware_version_big
+ && read_firmware_byte(10) == _firmware_version_little
+ && read_firmware_byte(11) == _has_compass
+ && read_firmware_byte(12) == _has_side_ir
+ && read_firmware_byte(13) == _has_base_ir
+ && read_firmware_byte(14) == _has_base_colour_sensor
+ && read_firmware_byte(15) == _has_top_colour_sensor
+ && read_firmware_byte(16) == _has_encoders
+ && read_firmware_byte(17) == _has_audio_pic
+ && read_firmware_byte(18) == _has_ultrasonic
+ && read_firmware_byte(19) == _has_temperature
+ && read_firmware_byte(20) == _has_recharging
+ && read_firmware_byte(21) == _has_433_radio
+ && read_firmware_byte(22) == _motor_calibration_set
+ && read_firmware_byte(29) == _base_ir_calibration_set
+ && read_firmware_byte(50) == _base_colour_calibration_set
+
+ ){
+ pc.printf("Flash successful.\n");
+ display.clear_display();
+ display.set_position(0,0);
+ display.write_string("FIRMWARE");
+ display.set_position(1,0);
+ display.write_string("UPDATED");
+ }
+ else {pc.printf("ERROR: Corrupt data. Flashing failed.\n");
+ display.clear_display();
+ display.set_position(0,0);
+ display.write_string("UPDATE");
+ display.set_position(1,0);
+ display.write_string("FAILED");
+ wait(1);
+ }
+
+ wait(0.5);
+ psi.debug("\nResetting...\n");
+ wait(0.2);
+ mbed_reset();
+}
+
+
+
+//PCB Features
+#define HAS_COMPASS 0
+#define HAS_SIDE_IR 1
+#define HAS_BASE_IR 1
+#define HAS_ULTRASONIC 1
+#define HAS_BASE_COLOUR_SENSOR 1
+#define HAS_TOP_COLOUR_SENSOR 0
+#define HAS_ENCODERS 0
+#define HAS_AUDIO_PIC 0
+#define HAS_TEMPERATURE 1
+#define HAS_RECHARGING 1
+#define HAS_433_RADIO 0
+
+//Calibration
+#define MOTOR_CALIBRATION_SET 0
\ No newline at end of file