Psi Swarm Robot
C++ Library for the PsiSwarm Robot - Version 0.8
Dependents: PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk
Fork of
PsiSwarmV7_CPP
by 
Psi Swarm Robot
Diff: serial.cpp
- 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()