diff --git a/software/ros_packages/rover_arm/msg/ArmControlMessage.msg b/software/ros_packages/rover_arm/msg/ArmControlMessage.msg index 8323c22..38ae114 100644 --- a/software/ros_packages/rover_arm/msg/ArmControlMessage.msg +++ b/software/ros_packages/rover_arm/msg/ArmControlMessage.msg @@ -6,4 +6,5 @@ float64 wrist_pitch float64 wrist_roll bool clear_faults -bool reset_controllers \ No newline at end of file +bool reset_controllers +bool calibrate \ No newline at end of file diff --git a/software/ros_packages/rover_arm/msg/ArmStatusMessage.msg b/software/ros_packages/rover_arm/msg/ArmStatusMessage.msg index e7c7417..c52ff73 100644 --- a/software/ros_packages/rover_arm/msg/ArmStatusMessage.msg +++ b/software/ros_packages/rover_arm/msg/ArmStatusMessage.msg @@ -5,12 +5,12 @@ float64 roll float64 wrist_pitch float64 wrist_roll -bool base_present -bool shoulder_present -bool elbow_present -bool roll_present -bool wrist_pitch_present -bool wrist_roll_present +int32 base_comm_status +int32 shoulder_comm_status +int32 elbow_comm_status +int32 roll_comm_status +int32 wrist_pitch_comm_status +int32 wrist_roll_comm_status int32 base_status int32 shoulder_status diff --git a/software/ros_packages/rover_arm/src/rover_arm.cpp b/software/ros_packages/rover_arm/src/rover_arm.cpp index 53ca02e..043a470 100644 --- a/software/ros_packages/rover_arm/src/rover_arm.cpp +++ b/software/ros_packages/rover_arm/src/rover_arm.cpp @@ -16,6 +16,10 @@ using namespace std; int device_address = 1; ////////// Global Variables ////////// +////////// Simplemotion variables and defines ///// +#define SMP_SERIAL_ENC_OFFSET 575 //This was missing from simplemotion_defs.h + +////////// ROS Variables and Defines ///// // ROS Parameter Defaults const string default_port = "/dev/ttyUSB0"; @@ -32,36 +36,36 @@ const double base_max_rev = 0.5; // Shoulder const smuint8 shoulder_address = 2; -const smint32 shoulder_counts_per_rev = 5725807; //FIXME +const smint32 shoulder_counts_per_rev = 2620130; const double shoulder_min_rev = -0.25; const double shoulder_max_rev = 0.25; //Elbow const smuint8 elbow_address = 3; -const smint32 elbow_counts_per_rev = 5725807; //FIXME +const smint32 elbow_counts_per_rev = 4917661; const double elbow_min_rev = -0.5; const double elbow_max_rev = 0.5; //Roll const smuint8 roll_address = 4; -const smint32 roll_counts_per_rev = 5725807; //FIXME +const smint32 roll_counts_per_rev = 1637581; const double roll_min_rev = -0.25; const double roll_max_rev = 0.25; //Wrist Pitch const smuint8 wrist_pitch_address = 5; -const smint32 wrist_pitch_counts_per_rev = 5725807; //FIXME +const smint32 wrist_pitch_counts_per_rev = 3799492; const double wrist_pitch_min_rev = -0.25; const double wrist_pitch_max_rev = 0.25; //Wrist Roll const smuint8 wrist_roll_address = 6; -const smint32 wrist_roll_counts_per_rev = 5725807; //FIXME +const smint32 wrist_roll_counts_per_rev = 3799492; -class RoverArm{ +class RoverArm { public: - RoverArm(int argc, char** argv){ + RoverArm(int argc, char **argv) { ros::init(argc, argv, "rover_arm"); node_handle = new ros::NodeHandle("~"); @@ -70,213 +74,279 @@ public: arm_bus_handle = smOpenBus(arm_port.c_str()); - if(arm_bus_handle < 0){ + if (arm_bus_handle < 0) { ROS_ERROR("Could not connect to arm"); return; - }else{ - arm_sucessfully_connected = true; + } else { + arm_successfully_connected = true; } - absolute_position_control_subscriber = node_handle->subscribe(default_absolute_position_control_topic, 1, &RoverArm::absolute_position_callback, this); - relative_position_control_subscriber = node_handle->subscribe(default_relative_position_control_topic, 1, &RoverArm::relative_position_callback, this); + absolute_position_control_subscriber = node_handle->subscribe(default_absolute_position_control_topic, 1, + &RoverArm::absolute_position_callback, this); + relative_position_control_subscriber = node_handle->subscribe(default_relative_position_control_topic, 1, + &RoverArm::relative_position_callback, this); status_publisher = node_handle->advertise(default_arm_status_topic, 1); } - void run(){ + void run() { char dir = 0; - - while(ros::ok()){ - if(!arm_sucessfully_connected){ return; } + while (ros::ok()) { + if (!arm_successfully_connected) { return; } clear_faults(); + set_calibration(); get_joint_statuses(); set_joint_positions(); reset_controllers(); ros::spinOnce(); - sleep(1); +// sleep(1); } ROS_ERROR("Shutting down."); } - void reset_controllers(){ - // Check if should reset - // Reset all, then kill thread + void reset_controllers() { + if (should_reset) { + smSetParameter(arm_bus_handle, base_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_RESTART); + received_first_joint_position_update = false; + should_clear_faults = true; + should_reset = false; + } } - void clear_faults(){ - if(should_clear_faults) { + void clear_faults() { + if (should_clear_faults) { smSetParameter(arm_bus_handle, base_address, SMP_FAULTS, 0); smSetParameter(arm_bus_handle, base_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + smSetParameter(arm_bus_handle, shoulder_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, shoulder_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + smSetParameter(arm_bus_handle, elbow_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, elbow_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + smSetParameter(arm_bus_handle, roll_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, roll_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + smSetParameter(arm_bus_handle, wrist_pitch_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, wrist_pitch_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + + smSetParameter(arm_bus_handle, wrist_roll_address, SMP_FAULTS, 0); + smSetParameter(arm_bus_handle, wrist_roll_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); + should_clear_faults = false; } } - void get_joint_statuses(){ + void set_calibration() { + if (should_calibrate) { - // Check if joint states valid - // SMP_STATUS - // SMP_FAULTS + // Base + smint32 base_current_offset; + smint32 base_position_to_offset; + smRead2Parameters(arm_bus_handle, base_address, SMP_SERIAL_ENC_OFFSET, &base_current_offset, + SMP_ACTUAL_POSITION_FB, &base_position_to_offset); - // SMP_ACTUAL_POSITION + smSetParameter(arm_bus_handle, base_address, SMP_SERIAL_ENC_OFFSET, base_current_offset - base_position_to_offset); - if(!received_first_joint_position_update) { - base_comm_state = smRead3Parameters(arm_bus_handle, base_address, SMP_STATUS, &base_status, SMP_FAULTS, - &base_faults, SMP_ACTUAL_POSITION_FB, &base_position); + // Shoulder + smint32 shoulder_current_offset; + smint32 shoulder_position_to_offset; + smRead2Parameters(arm_bus_handle, shoulder_address, SMP_SERIAL_ENC_OFFSET, &shoulder_current_offset, + SMP_ACTUAL_POSITION_FB, &shoulder_position_to_offset); + + smSetParameter(arm_bus_handle, shoulder_address, SMP_SERIAL_ENC_OFFSET, shoulder_current_offset - shoulder_position_to_offset); + + + // Elbow + + // Roll + + // Wrist Pitch + + // Wrist Roll + + // Save config through reboots + smSetParameter(arm_bus_handle, shoulder_address, SMP_SYSTEM_CONTROL, SMP_SYSTEM_CONTROL_SAVECFG); + + should_reset = true; + should_clear_faults = true; + should_calibrate = false; + } + } + + void get_joint_statuses() { + + base_comm_state = smRead3Parameters(arm_bus_handle, base_address, SMP_STATUS, &base_status, SMP_FAULTS, + &base_faults, SMP_ACTUAL_POSITION_FB, &base_current_position); + + + shoulder_comm_state = smRead3Parameters(arm_bus_handle, shoulder_address, SMP_STATUS, &shoulder_status, + SMP_FAULTS, + &shoulder_faults, SMP_ACTUAL_POSITION_FB, &shoulder_current_position); + + + elbow_comm_state = smRead3Parameters(arm_bus_handle, elbow_address, SMP_STATUS, &elbow_status, SMP_FAULTS, + &elbow_faults, SMP_ACTUAL_POSITION_FB, &elbow_current_position); + + + roll_comm_state = smRead3Parameters(arm_bus_handle, roll_address, SMP_STATUS, &roll_status, SMP_FAULTS, + &roll_faults, SMP_ACTUAL_POSITION_FB, &roll_current_position); + + + wrist_pitch_comm_state = smRead3Parameters(arm_bus_handle, wrist_pitch_address, SMP_STATUS, &wrist_pitch_status, + SMP_FAULTS, + &wrist_pitch_faults, SMP_ACTUAL_POSITION_FB, + &wrist_pitch_current_position); + + + wrist_roll_comm_state = smRead3Parameters(arm_bus_handle, wrist_roll_address, SMP_STATUS, &wrist_roll_status, + SMP_FAULTS, + &wrist_roll_faults, SMP_ACTUAL_POSITION_FB, + &wrist_roll_current_position); + + if (!received_first_joint_position_update) { + base_set_position = base_current_position; + shoulder_set_position = shoulder_current_position; + elbow_set_position = elbow_current_position; + roll_set_position = roll_current_position; + wrist_pitch_last_set_position = wrist_pitch_current_position; + wrist_pitch_set_position = wrist_pitch_current_position; + wrist_roll_set_position = wrist_roll_current_position; + + received_first_joint_position_update = true; } - smint32 base_current_position; - smRead3Parameters(arm_bus_handle, base_address, SMP_STATUS, &base_status, SMP_FAULTS, - &base_faults, SMP_ACTUAL_POSITION_FB, &base_current_position); + status_message.base = base_current_position / double(base_counts_per_rev); + status_message.shoulder = shoulder_current_position / double(shoulder_counts_per_rev); + status_message.elbow = elbow_current_position / double(elbow_counts_per_rev); + status_message.roll = roll_set_position / double(roll_counts_per_rev); + status_message.wrist_pitch = wrist_pitch_set_position / double(wrist_pitch_counts_per_rev); + status_message.wrist_roll = wrist_roll_set_position / double(wrist_roll_counts_per_rev); - smint32 shoulder_current_position; - smRead3Parameters(arm_bus_handle, base_address, SMP_STATUS, &base_status, SMP_FAULTS, - &base_faults, SMP_ACTUAL_POSITION_FB, &shoulder_current_position); -// shoulder_comm_state = smRead3Parameters(arm_bus_handle, shoulder_address, SMP_STATUS, &shoulder_status, SMP_FAULTS, &shoulder_faults, SMP_ACTUAL_POSITION_FB, &shoulder_position); -// elbow_comm_state = smRead3Parameters(arm_bus_handle, elbow_address, SMP_STATUS, &elbow_status, SMP_FAULTS, &elbow_faults, SMP_ACTUAL_POSITION_FB, &elbow_position); -// roll_comm_state = smRead3Parameters(arm_bus_handle, roll_address, SMP_STATUS, &roll_status, SMP_FAULTS, &roll_faults, SMP_ACTUAL_POSITION_FB, &roll_position); -// wrist_pitch_comm_state = smRead3Parameters(arm_bus_handle, wrist_pitch_address, SMP_STATUS, &wrist_pitch_status, SMP_FAULTS, &wrist_pitch_faults, SMP_ACTUAL_POSITION_FB, &wrist_pitch_position); -// wrist_roll_comm_state = smRead3Parameters(arm_bus_handle, wrist_roll_address, SMP_STATUS, &wrist_roll_status, SMP_FAULTS, &wrist_roll_faults, SMP_ACTUAL_POSITION_FB, &wrist_roll_position); - ROS_INFO("Base position: %ld", base_position); - // If so, broadcast - status_message.base = base_current_position / base_counts_per_rev; - status_message.shoulder = shoulder_current_position / shoulder_counts_per_rev; - status_message.elbow = elbow_position / elbow_counts_per_rev; - status_message.roll = roll_position / roll_counts_per_rev; - status_message.wrist_pitch = wrist_pitch_position / wrist_pitch_counts_per_rev; - status_message.wrist_roll = wrist_roll_position / wrist_roll_counts_per_rev; + status_message.base_comm_status = base_comm_state; + status_message.shoulder_comm_status = shoulder_comm_state; + status_message.elbow_comm_status = elbow_comm_state; + status_message.roll_comm_status = roll_comm_state; + status_message.wrist_pitch_comm_status = wrist_pitch_comm_state; + status_message.wrist_roll_comm_status = wrist_roll_comm_state; -// ROS_INFO("BASE:%d\tSHOULDER: %d\tELBOW: %d\tROLL: %d\tW_PITCH: %d\tW_ROLL: %d\t", base_comm_state, shoulder_comm_state, elbow_comm_state, roll_comm_state, wrist_pitch_comm_state, wrist_roll_comm_state ); + status_message.base_status = base_status; + status_message.shoulder_status = shoulder_status; + status_message.elbow_status = elbow_status; + status_message.roll_status = roll_status; + status_message.wrist_pitch_status = wrist_pitch_status; + status_message.wrist_roll_status = wrist_roll_status; - //float64 base - //float64 shoulder - //float64 elbow - //float64 roll - //float64 wrist_pitch - //float64 wrist_roll - // - //bool base_present - //bool shoulder_present - //bool elbow_present - //bool roll_present - //bool wrist_pitch_present - //bool wrist_roll_present - // - //int32 base_status - //int32 shoulder_status - //int32 elbow_status - //int32 roll_status - //int32 wrist_pitch_status - //int32 wrist_roll_status - // - //int32 base_faults - //int32 shoulder_faults - //int32 elbow_faults - //int32 roll_faults - //int32 wrist_pitch_faults - //int32 wrist_roll_faults + status_message.base_faults = base_faults; + status_message.shoulder_faults = shoulder_faults; + status_message.elbow_faults = elbow_faults; + status_message.roll_faults = roll_faults; + status_message.wrist_pitch_faults = wrist_pitch_faults; + status_message.wrist_roll_faults = wrist_roll_faults; status_publisher.publish(status_message); - // If first run, set received first message - if(!received_first_joint_position_update){ received_first_joint_position_update = true; } - } - void set_joint_positions(){ - if(new_positions_received){ + void set_joint_positions() { + if (new_positions_received) { set_base_position(); set_shoulder_position(); set_elbow_position(); set_roll_position(); set_wrist_positions(); - new_positions_received = false ; + new_positions_received = false; } } - void set_base_position(){ - smSetParameter(arm_bus_handle, base_address, SMP_FAULTS, 0); - smSetParameter(arm_bus_handle, base_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); - smSetParameter(arm_bus_handle, base_address, SMP_ABSOLUTE_SETPOINT, base_position); - ROS_INFO("SENT TO BASE: %ld", base_position); + void set_base_position() { + smSetParameter(arm_bus_handle, base_address, SMP_ABSOLUTE_SETPOINT, base_set_position); } - void set_shoulder_position(){ -// smSetParameter(arm_bus_handle, shoulder_address, SMP_FAULTS, 0); -// smSetParameter(arm_bus_handle, shoulder_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE); -// smSetParameter(arm_bus_handle, shoulder_address, SMP_ABSOLUTE_SETPOINT, shoulder_position); + void set_shoulder_position() { + smSetParameter(arm_bus_handle, shoulder_address, SMP_ABSOLUTE_SETPOINT, shoulder_set_position); } - void set_elbow_position(){ + void set_elbow_position() { + smSetParameter(arm_bus_handle, elbow_address, SMP_ABSOLUTE_SETPOINT, elbow_set_position); } - void set_roll_position(){ + void set_roll_position() { + smSetParameter(arm_bus_handle, roll_address, SMP_ABSOLUTE_SETPOINT, roll_set_position); + } + + void set_wrist_positions() { + smSetParameter(arm_bus_handle, wrist_roll_address, SMP_ABSOLUTE_SETPOINT, wrist_roll_set_position); + smSetParameter(arm_bus_handle, wrist_pitch_address, SMP_ABSOLUTE_SETPOINT, wrist_pitch_set_position); } - void set_wrist_positions(){ - - } - - void absolute_position_callback(const rover_arm::ArmControlMessage::ConstPtr& msg) - { - if(!received_first_joint_position_update){ return; } + void absolute_position_callback(const rover_arm::ArmControlMessage::ConstPtr &msg) { + if (!received_first_joint_position_update) { return; } + base_set_position = msg->base * base_counts_per_rev; + shoulder_set_position = msg->shoulder * shoulder_counts_per_rev; + elbow_set_position = msg->elbow * elbow_counts_per_rev; + roll_set_position = msg->roll * roll_counts_per_rev; -// base_position = smint32(min(max(msg->base, base_min), base_max_rev) * base_counts_per_rev); -// shoulder_position = smint32(min(max(msg->base, base_min), base_max_rev) * base_counts_per_rev);; -// elbow_position = smint32(min(max(msg->elbow, elbow_min_rev), elbow_max_rev) * elbow_counts_per_rev);; -// roll_position = smint32(min(max(msg->roll, roll_min_rev), roll_max_rev) * roll_counts_per_rev);; -// wrist_pitch_position = smint32(min(max(msg->wrist_pitch, wrist_pitch_min_rev), wrist_pitch_max_rev) * wrist_pitch_counts_per_rev); -// wrist_roll_position = smint32(msg->wrist_roll * wrist_roll_counts_per_rev); + wrist_roll_set_position = msg->wrist_roll * wrist_roll_counts_per_rev; - base_position = msg->base * base_counts_per_rev; - shoulder_position = msg->base * base_counts_per_rev; + wrist_pitch_set_position = msg->wrist_pitch * wrist_pitch_counts_per_rev; + + smint32 roll_pitch_position_difference = (wrist_pitch_set_position - wrist_pitch_last_set_position) / 2; +// wrist_roll_set_position -= roll_pitch_position_difference; + wrist_pitch_last_set_position = wrist_pitch_set_position; -// shoulder_position = smint32(min(max(msg->base* base_counts_per_rev);; -// elbow_position = smint32(min(max(msg->elbow, elbow_min_rev), elbow_max_rev) * elbow_counts_per_rev);; -// roll_position = smint32(min(max(msg->roll, roll_min_rev), roll_max_rev) * roll_counts_per_rev);; -// wrist_pitch_position = smint32(min(max(msg->wrist_pitch, wrist_pitch_min_rev), wrist_pitch_max_rev) * wrist_pitch_counts_per_rev); -// wrist_roll_position = smint32(msg->wrist_roll * wrist_roll_counts_per_rev); should_clear_faults = (msg->clear_faults); should_reset = (msg->reset_controllers); + should_calibrate = msg->calibrate; - new_positions_received = true; - - ROS_INFO("BASE:%d\tSHOULDER: %d\tELBOW: %d\tROLL: %d\tW_PITCH: %d\tW_ROLL: %d\t", base_position, shoulder_position, elbow_position, roll_position, wrist_pitch_position, wrist_roll_position ); -// ROS_INFO("ABS::::Base: %f\tshoulder: %f", msg->base, msg->shoulder); + if(!should_calibrate && !should_reset){ + new_positions_received = true; + } } - void relative_position_callback(const rover_arm::ArmControlMessage::ConstPtr& msg) - { - if(!received_first_joint_position_update){ return; } + void relative_position_callback(const rover_arm::ArmControlMessage::ConstPtr &msg) { + if (!received_first_joint_position_update) { return; } - base_position += msg->base; - shoulder_position += msg->shoulder; - elbow_position += msg->elbow; - roll_position += msg->roll; - wrist_pitch_position += msg->wrist_pitch; - wrist_roll_position += msg->wrist_roll; + ROS_INFO("Shoulder before: %ld", shoulder_set_position); - should_clear_faults += msg->clear_faults; - should_reset += msg->reset_controllers; + base_set_position += msg->base * base_counts_per_rev; + shoulder_set_position += msg->shoulder * shoulder_counts_per_rev; + elbow_set_position += msg->elbow * elbow_counts_per_rev; + roll_set_position += msg->roll * roll_counts_per_rev; - new_positions_received = true; + wrist_roll_set_position += msg->wrist_roll * wrist_roll_counts_per_rev; - ROS_INFO("REL::::Base: %f\tshoulder: %f", msg->base, msg->shoulder); + wrist_pitch_set_position += msg->wrist_pitch * wrist_pitch_counts_per_rev; + + smint32 roll_pitch_position_difference = (wrist_pitch_set_position - wrist_pitch_last_set_position) / 2; +// wrist_roll_set_position -= roll_pitch_position_difference; + wrist_pitch_last_set_position = wrist_pitch_set_position; + + + should_clear_faults = (msg->clear_faults); + should_reset = (msg->reset_controllers); + should_calibrate = msg->calibrate; + + if(!should_calibrate && !should_reset) { + new_positions_received = true; + } + + ROS_INFO("Shoulder after: %ld", shoulder_set_position); } private: @@ -284,7 +354,7 @@ private: string arm_port; smbus arm_bus_handle; - bool arm_sucessfully_connected = false; + bool arm_successfully_connected = false; ros::Publisher status_publisher; rover_arm::ArmStatusMessage status_message; @@ -296,47 +366,52 @@ private: bool new_positions_received = false; - smint32 base_position = 0; + smint32 base_set_position = 0; + smint32 base_current_position = 0; smint32 base_comm_state = 0; smint32 base_status = 0; smint32 base_faults = 0; - smint32 shoulder_position = 0; + smint32 shoulder_set_position = 0; + smint32 shoulder_current_position = 0; smint32 shoulder_comm_state = 0; smint32 shoulder_status = 0; smint32 shoulder_faults = 0; - smint32 elbow_position = 0; + smint32 elbow_set_position = 0; + smint32 elbow_current_position = 0; smint32 elbow_comm_state = 0; smint32 elbow_status = 0; smint32 elbow_faults = 0; - smint32 roll_position = 0; + smint32 roll_set_position = 0; + smint32 roll_current_position = 0; smint32 roll_comm_state = 0; smint32 roll_status = 0; smint32 roll_faults = 0; - smint32 wrist_pitch_position = 0; + smint32 wrist_pitch_last_set_position = 0; + smint32 wrist_pitch_set_position = 0; + smint32 wrist_pitch_current_position = 0; smint32 wrist_pitch_comm_state = 0; smint32 wrist_pitch_status = 0; smint32 wrist_pitch_faults = 0; - smint32 wrist_roll_position = 0; + smint32 wrist_roll_set_position = 0; + smint32 wrist_roll_current_position = 0; smint32 wrist_roll_comm_state = 0; smint32 wrist_roll_status = 0; smint32 wrist_roll_faults = 0; - - + bool should_calibrate = false; bool should_clear_faults = true; bool should_reset = false; - }; -int main(int argc, char** argv){ +int main(int argc, char **argv) { RoverArm rover_arm(argc, argv); rover_arm.run(); } \ No newline at end of file