24 #include <interfaces/NaoSensorInterface.h>
26 #include <core/exceptions/software.h>
44 NaoSensorInterface::NaoSensorInterface() : Interface()
46 data_size =
sizeof(NaoSensorInterface_data_t);
48 data = (NaoSensorInterface_data_t *)
data_ptr;
75 add_fieldinfo(
IFT_ENUM,
"ultrasonic_direction", 1, &data->ultrasonic_direction,
"UltrasonicDirection");
88 unsigned char tmp_hash[] = {0x41, 0x41, 0x54, 0x94, 0xca, 0xe8, 0x3d, 0x7a, 0xb8, 0xaa, 0xc2, 0x4e, 0x2c, 0xac, 0xcb, 0x15};
93 NaoSensorInterface::~NaoSensorInterface()
111 default:
return "UNKNOWN";
122 return data->accel_x;
142 data->accel_x = new_accel_x;
153 return data->accel_y;
173 data->accel_y = new_accel_y;
184 return data->accel_z;
204 data->accel_z = new_accel_z;
235 data->gyro_x = new_gyro_x;
266 data->gyro_y = new_gyro_y;
277 return data->gyro_ref;
297 data->gyro_ref = new_gyro_ref;
308 return data->angle_x;
328 data->angle_x = new_angle_x;
339 return data->angle_y;
359 data->angle_y = new_angle_y;
370 return data->l_fsr_fl;
390 data->l_fsr_fl = new_l_fsr_fl;
401 return data->l_fsr_fr;
421 data->l_fsr_fr = new_l_fsr_fr;
432 return data->l_fsr_rl;
452 data->l_fsr_rl = new_l_fsr_rl;
463 return data->l_fsr_rr;
483 data->l_fsr_rr = new_l_fsr_rr;
494 return data->r_fsr_fl;
514 data->r_fsr_fl = new_r_fsr_fl;
525 return data->r_fsr_fr;
545 data->r_fsr_fr = new_r_fsr_fr;
556 return data->r_fsr_rl;
576 data->r_fsr_rl = new_r_fsr_rl;
587 return data->r_fsr_rr;
607 data->r_fsr_rr = new_r_fsr_rr;
618 return data->l_total_weight;
638 data->l_total_weight = new_l_total_weight;
649 return data->r_total_weight;
669 data->r_total_weight = new_r_total_weight;
680 return data->l_cop_x;
700 data->l_cop_x = new_l_cop_x;
711 return data->l_cop_y;
731 data->l_cop_y = new_l_cop_y;
742 return data->r_cop_x;
762 data->r_cop_x = new_r_cop_x;
773 return data->r_cop_y;
793 data->r_cop_y = new_r_cop_y;
807 return data->ultrasonic_distance_left;
823 throw Exception(
"Index value %u out of bounds (0..4)", index);
825 return data->ultrasonic_distance_left[index];
848 memcpy(data->ultrasonic_distance_left, new_ultrasonic_distance_left,
sizeof(
float) * 4);
864 throw Exception(
"Index value %u out of bounds (0..4)", index);
866 data->ultrasonic_distance_left[index] = new_ultrasonic_distance_left;
879 return data->ultrasonic_distance_right;
895 throw Exception(
"Index value %u out of bounds (0..4)", index);
897 return data->ultrasonic_distance_right[index];
920 memcpy(data->ultrasonic_distance_right, new_ultrasonic_distance_right,
sizeof(
float) * 4);
936 throw Exception(
"Index value %u out of bounds (0..4)", index);
938 data->ultrasonic_distance_right[index] = new_ultrasonic_distance_right;
972 data->ultrasonic_direction = new_ultrasonic_direction;
983 return data->l_foot_bumper_l;
1003 data->l_foot_bumper_l = new_l_foot_bumper_l;
1014 return data->l_foot_bumper_r;
1034 data->l_foot_bumper_r = new_l_foot_bumper_r;
1045 return data->r_foot_bumper_l;
1065 data->r_foot_bumper_l = new_r_foot_bumper_l;
1076 return data->r_foot_bumper_r;
1096 data->r_foot_bumper_r = new_r_foot_bumper_r;
1107 return data->head_touch_front;
1127 data->head_touch_front = new_head_touch_front;
1138 return data->head_touch_middle;
1158 data->head_touch_middle = new_head_touch_middle;
1169 return data->head_touch_rear;
1189 data->head_touch_rear = new_head_touch_rear;
1200 return data->chest_button;
1220 data->chest_button = new_chest_button;
1231 return data->battery_charge;
1251 data->battery_charge = new_battery_charge;
1259 if ( strncmp(
"EmitUltrasonicWaveMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
1261 }
else if ( strncmp(
"StartUltrasonicMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
1263 }
else if ( strncmp(
"StopUltrasonicMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
1267 "message type for this interface type.", type);
1283 memcpy(data, oi->data,
sizeof(NaoSensorInterface_data_t));
1289 if (strcmp(enumtype,
"UltrasonicDirection") == 0) {
1308 data_size =
sizeof(EmitUltrasonicWaveMessage_data_t);
1311 data = (EmitUltrasonicWaveMessage_data_t *)
data_ptr;
1313 data->ultrasonic_direction = ini_ultrasonic_direction;
1314 add_fieldinfo(
IFT_ENUM,
"ultrasonic_direction", 1, &data->ultrasonic_direction,
"UltrasonicDirection");
1319 data_size =
sizeof(EmitUltrasonicWaveMessage_data_t);
1322 data = (EmitUltrasonicWaveMessage_data_t *)
data_ptr;
1324 add_fieldinfo(
IFT_ENUM,
"ultrasonic_direction", 1, &data->ultrasonic_direction,
"UltrasonicDirection");
1341 data = (EmitUltrasonicWaveMessage_data_t *)
data_ptr;
1377 data->ultrasonic_direction = new_ultrasonic_direction;
1402 data_size =
sizeof(StartUltrasonicMessage_data_t);
1405 data = (StartUltrasonicMessage_data_t *)
data_ptr;
1407 data->ultrasonic_direction = ini_ultrasonic_direction;
1408 add_fieldinfo(
IFT_ENUM,
"ultrasonic_direction", 1, &data->ultrasonic_direction,
"UltrasonicDirection");
1413 data_size =
sizeof(StartUltrasonicMessage_data_t);
1416 data = (StartUltrasonicMessage_data_t *)
data_ptr;
1418 add_fieldinfo(
IFT_ENUM,
"ultrasonic_direction", 1, &data->ultrasonic_direction,
"UltrasonicDirection");
1435 data = (StartUltrasonicMessage_data_t *)
data_ptr;
1471 data->ultrasonic_direction = new_ultrasonic_direction;
1494 data_size =
sizeof(StopUltrasonicMessage_data_t);
1497 data = (StopUltrasonicMessage_data_t *)
data_ptr;
1515 data = (StopUltrasonicMessage_data_t *)
data_ptr;