24 #include <interfaces/MotorInterface.h>
26 #include <core/exceptions/software.h>
60 MotorInterface::MotorInterface() : Interface()
62 data_size =
sizeof(MotorInterface_data_t);
64 data = (MotorInterface_data_t *)
data_ptr;
91 unsigned char tmp_hash[] = {0x4d, 0x9b, 0xb8, 0x1e, 0xb7, 0x9e, 0xc1, 0xea, 0xc6, 0xad, 0xf9, 0x6, 0xd7, 0x6d, 0xa4, 0xa5};
96 MotorInterface::~MotorInterface()
110 return data->motor_state;
132 data->motor_state = new_motor_state;
145 return data->drive_mode;
167 data->drive_mode = new_drive_mode;
180 return data->right_rpm;
202 data->right_rpm = new_right_rpm;
215 return data->rear_rpm;
237 data->rear_rpm = new_rear_rpm;
250 return data->left_rpm;
272 data->left_rpm = new_left_rpm;
285 return data->odometry_path_length;
307 data->odometry_path_length = new_odometry_path_length;
320 return data->odometry_position_x;
342 data->odometry_position_x = new_odometry_position_x;
355 return data->odometry_position_y;
377 data->odometry_position_y = new_odometry_position_y;
390 return data->odometry_orientation;
412 data->odometry_orientation = new_odometry_orientation;
517 data->omega = new_omega;
531 return data->controller;
554 data->controller = new_controller;
569 return data->controller_thread_name;
593 strncpy(data->controller_thread_name, new_controller_thread_name,
sizeof(data->controller_thread_name));
601 if ( strncmp(
"SetMotorStateMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
603 }
else if ( strncmp(
"AcquireControlMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
605 }
else if ( strncmp(
"ResetOdometryMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
607 }
else if ( strncmp(
"DriveRPMMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
609 }
else if ( strncmp(
"GotoMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
611 }
else if ( strncmp(
"TransMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
613 }
else if ( strncmp(
"RotMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
615 }
else if ( strncmp(
"TransRotMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
617 }
else if ( strncmp(
"OrbitMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
619 }
else if ( strncmp(
"LinTransRotMessage", type, __INTERFACE_MESSAGE_TYPE_SIZE) == 0 ) {
623 "message type for this interface type.", type);
639 memcpy(data, oi->data,
sizeof(MotorInterface_data_t));
661 data_size =
sizeof(SetMotorStateMessage_data_t);
664 data = (SetMotorStateMessage_data_t *)
data_ptr;
666 data->motor_state = ini_motor_state;
672 data_size =
sizeof(SetMotorStateMessage_data_t);
675 data = (SetMotorStateMessage_data_t *)
data_ptr;
694 data = (SetMotorStateMessage_data_t *)
data_ptr;
708 return data->motor_state;
730 data->motor_state = new_motor_state;
756 data_size =
sizeof(AcquireControlMessage_data_t);
759 data = (AcquireControlMessage_data_t *)
data_ptr;
761 data->controller = ini_controller;
762 strncpy(data->controller_thread_name, ini_controller_thread_name, 64);
769 data_size =
sizeof(AcquireControlMessage_data_t);
772 data = (AcquireControlMessage_data_t *)
data_ptr;
792 data = (AcquireControlMessage_data_t *)
data_ptr;
807 return data->controller;
830 data->controller = new_controller;
844 return data->controller_thread_name;
868 strncpy(data->controller_thread_name, new_controller_thread_name,
sizeof(data->controller_thread_name));
891 data_size =
sizeof(ResetOdometryMessage_data_t);
894 data = (ResetOdometryMessage_data_t *)
data_ptr;
912 data = (ResetOdometryMessage_data_t *)
data_ptr;
941 data_size =
sizeof(DriveRPMMessage_data_t);
944 data = (DriveRPMMessage_data_t *)
data_ptr;
946 data->front_right = ini_front_right;
947 data->front_left = ini_front_left;
948 data->rear = ini_rear;
956 data_size =
sizeof(DriveRPMMessage_data_t);
959 data = (DriveRPMMessage_data_t *)
data_ptr;
980 data = (DriveRPMMessage_data_t *)
data_ptr;
992 return data->front_right;
1012 data->front_right = new_front_right;
1022 return data->front_left;
1042 data->front_left = new_front_left;
1072 data->rear = new_rear;
1103 data = (GotoMessage_data_t *)
data_ptr;
1107 data->phi = ini_phi;
1108 data->time_sec = ini_time_sec;
1120 data = (GotoMessage_data_t *)
data_ptr;
1142 data = (GotoMessage_data_t *)
data_ptr;
1234 data->phi = new_phi;
1244 return data->time_sec;
1264 data->time_sec = new_time_sec;
1290 data_size =
sizeof(TransMessage_data_t);
1293 data = (TransMessage_data_t *)
data_ptr;
1303 data_size =
sizeof(TransMessage_data_t);
1306 data = (TransMessage_data_t *)
data_ptr;
1326 data = (TransMessage_data_t *)
data_ptr;
1416 data = (RotMessage_data_t *)
data_ptr;
1418 data->omega = ini_omega;
1427 data = (RotMessage_data_t *)
data_ptr;
1446 data = (RotMessage_data_t *)
data_ptr;
1478 data->omega = new_omega;
1505 data_size =
sizeof(TransRotMessage_data_t);
1508 data = (TransRotMessage_data_t *)
data_ptr;
1512 data->omega = ini_omega;
1520 data_size =
sizeof(TransRotMessage_data_t);
1523 data = (TransRotMessage_data_t *)
data_ptr;
1544 data = (TransRotMessage_data_t *)
data_ptr;
1636 data->omega = new_omega;
1663 data_size =
sizeof(OrbitMessage_data_t);
1666 data = (OrbitMessage_data_t *)
data_ptr;
1670 data->omega = ini_omega;
1678 data_size =
sizeof(OrbitMessage_data_t);
1681 data = (OrbitMessage_data_t *)
data_ptr;
1702 data = (OrbitMessage_data_t *)
data_ptr;
1794 data->omega = new_omega;
1821 data_size =
sizeof(LinTransRotMessage_data_t);
1824 data = (LinTransRotMessage_data_t *)
data_ptr;
1828 data->omega = ini_omega;
1836 data_size =
sizeof(LinTransRotMessage_data_t);
1839 data = (LinTransRotMessage_data_t *)
data_ptr;
1860 data = (LinTransRotMessage_data_t *)
data_ptr;
1952 data->omega = new_omega;