C++ Library for the PsiSwarm Robot - Version 0.8

Dependents:   PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk

Fork of PsiSwarmV7_CPP by Psi Swarm Robot

Revision:
10:e58323951c08
Parent:
9:dde9e21030eb
Child:
11:312663037b8c
--- a/serial.cpp	Sun Oct 16 14:12:49 2016 +0000
+++ b/serial.cpp	Sun Oct 16 16:00:20 2016 +0000
@@ -1,11 +1,11 @@
 /* University of York Robotics Laboratory PsiSwarm Library: Serial Control Source File
- * 
+ *
  * Copyright 2016 University of York
  *
- * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. 
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
  * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and limitations under the License.
  *
  * File: serial.cpp
@@ -73,7 +73,7 @@
 void IF_invalid_transfer(void)
 {
     debug("File transfer failed\n");
-    if(data_written == 1){
+    if(data_written == 1) {
         debug("Deleting corrupted file\n");
         remove(filename);
     }
@@ -177,12 +177,12 @@
                 expected_size = block_size;
                 if(block_index == final_block) expected_size = file_length % block_size;
                 if(expected_size == 0) expected_size = block_size;
-                if(length!=expected_size + 1){
-                    // Unexpected length   
+                if(length!=expected_size + 1) {
+                    // Unexpected length
                     debug("File data unexpected length in packet %d (%d bytes received, %d bytes expected)\n",block_index,length-1,expected_size);
-                }else{
-                    char transfer_mode[2]={'a'};
-                    if(block_index == 1){
+                } else {
+                    char transfer_mode[2]= {'a'};
+                    if(block_index == 1) {
                         transfer_mode[0]='w';
                     }
                     FILE *fp = fopen(filename,transfer_mode);
@@ -194,14 +194,13 @@
                     fclose(fp);
                     if(data_written == false && bytes_written > 0) data_written = true;
                     debug("Bytes written: %d\n",expected_size * bytes_written);
-                    if(block_index < final_block){
-                    debug("Message packet %d received and written\n",block_index);
-                    //Send acknowledge ("D")
-                    ft_timeout.detach();
-                    ft_timeout.attach(IF_file_transfer_timeout,1.0);   
-                    bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,1,"D");
-                    }
-                    else{
+                    if(block_index < final_block) {
+                        debug("Message packet %d received and written\n",block_index);
+                        //Send acknowledge ("D")
+                        ft_timeout.detach();
+                        ft_timeout.attach(IF_file_transfer_timeout,1.0);
+                        bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,1,"D");
+                    } else {
                         //Last data block written
                         //[Put file checking code here]
                         //Send acknowledge ("P");
@@ -211,11 +210,11 @@
                         wait(0.25);
                         //Calculate CRC16 value for file
                         IF_calculateCRC16(file_length);
-                        
+
                         display.clear_display();
                         display.write_string("FILE TRANSFER");
                         display.set_position(1,0);
-                        display.write_string("COMPLETE");   
+                        display.write_string("COMPLETE");
                         wait(1);
                         debug("File transfer mode ended\n");
                         IF_end_file_transfer_mode();
@@ -561,7 +560,7 @@
                         display.write_string("CONNECTED");
                     } else command_status = 2;
                     break;
-                 case 6:
+                case 6:
                     strcpy(subcommand,"MESSAGE 6");
                     if(allow_commands) {
                         command_status = 1;
@@ -571,7 +570,7 @@
                         display.set_position(1,0);
                         display.write_string("DISCONNECTED");
                     } else command_status = 2;
-                    break;    
+                    break;
             }
             break;
         case 21:
@@ -586,7 +585,7 @@
                 command_status = 3;
             }
             break;
-        case 22:
+        case 22: {
             strcpy(command,"PRINT CHARACTERS ");
             char print_message[2];
             print_message[0]=message[1];
@@ -596,6 +595,7 @@
                 display.write_string(print_message,2);
             } else command_status = 2;
             break;
+        }
         case 23:
             strcpy(command,"SET DISPLAY B.NESS");
             dec = IF_decode_unsigned_float(message[1],message[2]);
@@ -806,100 +806,100 @@
             strcpy(command,"STORE BG. IR VALUES");
             if(allow_commands) {
                 command_status = 1;
-                store_background_raw_ir_values();
+                sensors.store_background_raw_ir_values();
             } else command_status = 2;
             break;
         case 81:
             strcpy(command,"STORE IL. IR VALUES");
             if(allow_commands) {
                 command_status = 1;
-                store_illuminated_raw_ir_values();
+                sensors.store_illuminated_raw_ir_values();
             } else command_status = 2;
             break;
         case 82:
             strcpy(command,"STORE IR VALUES");
             if(allow_commands) {
                 command_status = 1;
-                store_ir_values();
+                sensors.store_ir_values();
             } else command_status = 2;
             break;
         case 83:
             strcpy(command,"STORE BG BASE IR VALUES");
             if(allow_commands) {
                 command_status = 1;
-                store_background_base_ir_values();
+                sensors.store_background_base_ir_values();
             } else command_status = 2;
             break;
         case 84:
             strcpy(command,"STORE IL. BASE IR VALUES");
             if(allow_commands) {
                 command_status = 1;
-                store_illuminated_base_ir_values();
+                sensors.store_illuminated_base_ir_values();
             } else command_status = 2;
             break;
         case 85:
             strcpy(command,"STORE BASE IR VALUES");
             if(allow_commands) {
                 command_status = 1;
-                store_base_ir_values();
+                sensors.store_base_ir_values();
             } else command_status = 2;
             break;
         case 86:
             strcpy(command,"STORE ALL IR VALUES");
             if(allow_commands) {
                 command_status = 1;
-                store_ir_values();
-                store_base_ir_values();
+                sensors.store_ir_values();
+                sensors.store_base_ir_values();
             } else command_status = 2;
             break;
         case 90:
             sprintf(command,"%s %d","GET BG IR VALUE",message[1]);
-            sprintf(ret_message,"%d",get_background_raw_ir_value(message[1]));
+            sprintf(ret_message,"%d",sensors.get_background_raw_ir_value(message[1]));
             send_message = 1;
             break;
         case 91:
             sprintf(command,"%s %d","GET IL IR VALUE",message[1]);
-            sprintf(ret_message,"%d",get_illuminated_raw_ir_value(message[1]));
+            sprintf(ret_message,"%d",sensors.get_illuminated_raw_ir_value(message[1]));
             send_message = 1;
             break;
         case 92:
             strcpy(command,"GET BG IR VALUES");
-            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",get_background_raw_ir_value(0),get_background_raw_ir_value(1),get_background_raw_ir_value(2),get_background_raw_ir_value(3),get_background_raw_ir_value(4),get_background_raw_ir_value(5),get_background_raw_ir_value(6),get_background_raw_ir_value(7));
+            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",sensors.get_background_raw_ir_value(0),sensors.get_background_raw_ir_value(1),sensors.get_background_raw_ir_value(2),sensors.get_background_raw_ir_value(3),sensors.get_background_raw_ir_value(4),sensors.get_background_raw_ir_value(5),sensors.get_background_raw_ir_value(6),sensors.get_background_raw_ir_value(7));
             send_message = 1;
             break;
         case 93:
             strcpy(command,"GET ILLUMINATED IR VALUES");
-            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",get_illuminated_raw_ir_value(0),get_illuminated_raw_ir_value(1),get_illuminated_raw_ir_value(2),get_illuminated_raw_ir_value(3),get_illuminated_raw_ir_value(4),get_illuminated_raw_ir_value(5),get_illuminated_raw_ir_value(6),get_illuminated_raw_ir_value(7));
+            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",sensors.get_illuminated_raw_ir_value(0),sensors.get_illuminated_raw_ir_value(1),sensors.get_illuminated_raw_ir_value(2),sensors.get_illuminated_raw_ir_value(3),sensors.get_illuminated_raw_ir_value(4),sensors.get_illuminated_raw_ir_value(5),sensors.get_illuminated_raw_ir_value(6),sensors.get_illuminated_raw_ir_value(7));
             send_message = 1;
             break;
         case 94:
             sprintf(command,"%s %d","GET BG BASE IR VALUE",message[1]);
-            sprintf(ret_message,"%d",get_background_base_ir_value(message[1]));
+            sprintf(ret_message,"%d",sensors.get_background_base_ir_value(message[1]));
             send_message = 1;
             break;
         case 95:
             sprintf(command,"%s %d","GET IL BASE IR VALUE",message[1]);
-            sprintf(ret_message,"%d",get_illuminated_base_ir_value(message[1]));
+            sprintf(ret_message,"%d",sensors.get_illuminated_base_ir_value(message[1]));
             send_message = 1;
             break;
         case 96:
             strcpy(command,"GET BG BASE IR VALUES");
-            sprintf(ret_message,"%03X%03X%03X%03X%03X",get_background_base_ir_value(0),get_background_base_ir_value(1),get_background_base_ir_value(2),get_background_base_ir_value(3),get_background_base_ir_value(4));
+            sprintf(ret_message,"%03X%03X%03X%03X%03X",sensors.get_background_base_ir_value(0),sensors.get_background_base_ir_value(1),sensors.get_background_base_ir_value(2),sensors.get_background_base_ir_value(3),sensors.get_background_base_ir_value(4));
             send_message = 1;
             break;
         case 97:
             strcpy(command,"GET IL BASE IR VALUES");
-            sprintf(ret_message,"%03X%03X%03X%03X%03X",get_illuminated_base_ir_value(0),get_illuminated_base_ir_value(1),get_illuminated_base_ir_value(2),get_illuminated_base_ir_value(3),get_illuminated_base_ir_value(4));
+            sprintf(ret_message,"%03X%03X%03X%03X%03X",sensors.get_illuminated_base_ir_value(0),sensors.get_illuminated_base_ir_value(1),sensors.get_illuminated_base_ir_value(2),sensors.get_illuminated_base_ir_value(3),sensors.get_illuminated_base_ir_value(4));
             send_message = 1;
             break;
         case 98:
             strcpy(command,"CALCULATE BASE IR VALUES");
-            sprintf(ret_message,"%03X%03X%03X%03X%03X",calculate_base_ir_value(0),calculate_base_ir_value(1),calculate_base_ir_value(2),calculate_base_ir_value(3),calculate_base_ir_value(4));
+            sprintf(ret_message,"%03X%03X%03X%03X%03X",sensors.calculate_base_ir_value(0),sensors.calculate_base_ir_value(1),sensors.calculate_base_ir_value(2),sensors.calculate_base_ir_value(3),sensors.calculate_base_ir_value(4));
             send_message = 1;
             break;
         case 99:
             strcpy(command,"CALCULATE SIDE IR VALUES");
-            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",calculate_side_ir_value(0),calculate_side_ir_value(1),calculate_side_ir_value(2),calculate_side_ir_value(3),calculate_side_ir_value(4),calculate_side_ir_value(5),calculate_side_ir_value(6),calculate_side_ir_value(7));
+            sprintf(ret_message,"%03X%03X%03X%03X%03X%03X%03X%03X",sensors.calculate_side_ir_value(0),sensors.calculate_side_ir_value(1),sensors.calculate_side_ir_value(2),sensors.calculate_side_ir_value(3),sensors.calculate_side_ir_value(4),sensors.calculate_side_ir_value(5),sensors.calculate_side_ir_value(6),sensors.calculate_side_ir_value(7));
             send_message = 1;
             break;
         case 100:
@@ -976,7 +976,7 @@
             sprintf(ret_message,"%d",has_433_radio);
             send_message = 1;
             break;
-        case 123:
+        case 123: {
             strcpy(command,"GET FIRMWARE H-DESC");
             char byte0 = 0;
             char byte1 = 1;
@@ -994,6 +994,7 @@
             sprintf(ret_message,"%c%c",byte0,byte1);
             send_message = 1;
             break;
+        }
         case 124:
             strcpy(command,"GET PCB VERSION");
             sprintf(ret_message,"%1.2f",pcb_version);
@@ -1179,7 +1180,7 @@
 }
 
 Timeout bt_message_timeout;
-static float bt_message_timeout_period = 0.001; // 1 millisecond
+//static float bt_message_timeout_period = 0.001; // 1 millisecond
 char bt_buffer[255];
 int bt_buffer_index = 0;
 
@@ -1217,12 +1218,14 @@
 //}
 
 
-void IF_set_filename(char * filename_in){
+void IF_set_filename(char * filename_in)
+{
     strcpy(filename,filename_in);
 }
- 
-unsigned short IF_calculateCRC16(int file_length){
-    unsigned short crc16table[256] = { 
+
+unsigned short IF_calculateCRC16(int file_length)
+{
+    unsigned short crc16table[256] = {
         0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
         0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
         0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
@@ -1238,7 +1241,7 @@
         0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
         0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
         0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
-        0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, 
+        0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
         0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
         0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
         0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
@@ -1254,8 +1257,9 @@
         0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
         0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
         0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
-        0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202};
-        
+        0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
+    };
+
     //Opens, reads and calculates the CRC16 value for file pointed to by filename
     unsigned short crc_value = 0;
     FILE *fp = fopen(filename,"r");
@@ -1265,26 +1269,26 @@
     buffer = (char*) malloc (sizeof(char)*limit);
     int blocks = 1;
     if(file_length > limit) blocks += file_length / limit;
-    for(int i=0;i<blocks;i++){
+    for(int i=0; i<blocks; i++) {
         //Determine size of this block
         int blocksize = limit;
-        if(i == blocks-1){
-            if((file_length % limit) != 0) blocksize = file_length % limit;   
-        }               
+        if(i == blocks-1) {
+            if((file_length % limit) != 0) blocksize = file_length % limit;
+        }
         debug("Calculating %d bytes of CRC data...\n",blocksize);
         int result;
         result = fread(buffer,1,blocksize,fp);
         debug("Data read: %d\n",result);
-        for(int j=0;j<blocksize;j++){
+        for(int j=0; j<blocksize; j++) {
             int subindex = ((crc_value>>8)^*(char *)(buffer[j]))&0x00FF;
             //debug("J:%d Subindex:%d\n",j,subindex);
             unsigned short table_value = crc16table[subindex];
-            crc_value=(crc_value<<8)^table_value;   
+            crc_value=(crc_value<<8)^table_value;
         }
     }
     fclose(fp);
     debug("CRC Calculated: %x\n",crc_value);
-    return crc_value;               
+    return crc_value;
 }
 
 void IF_bt_rx_callback()