caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-09 02:31:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Rover arm now moves one axis via topics. Close to having the rest moving tomorrow.

Browse Source
This commit is contained in:
Corwin Perren
2018-08-04 01:28:21 -07:00
parent d62fdd45b3
commit 7f80f44b96
10 changed files with 406 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
software/ros_packages/rover_arm/CMakeLists.txt
View File

@@ -9,8 +9,9 @@ add_compile_options(-std=c++11)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
#message_generation
std_msgs
std_msgs
message_generation
)
## System dependencies are found with CMake's conventions
@@ -47,9 +48,11 @@ find_package(catkin REQUIRED COMPONENTS
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
#add_message_files(
# FILES
#)
add_message_files(
FILES
ArmControlMessage.msg
ArmStatusMessage.msg
)
## Generate services in the 'srv' folder
# add_service_files(
@@ -66,10 +69,10 @@ find_package(catkin REQUIRED COMPONENTS
# )
## Generate added messages and services with any dependencies listed here
#generate_messages(
# DEPENDENCIES
# std_msgs
#)
generate_messages(
DEPENDENCIES
std_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
@@ -103,7 +106,7 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES rover_arm
CATKIN_DEPENDS roscpp std_msgs
CATKIN_DEPENDS roscpp std_msgs message_generation
# DEPENDS system_lib
)
@@ -150,7 +153,7 @@ add_executable(${PROJECT_NAME} src/rover_arm.cpp )
target_link_libraries(
${PROJECT_NAME}
${catkin_LIBRARIES}
simplemotion
simplemotion2
)
#############

9
software/ros_packages/rover_arm/msg/ArmControlMessage.msg Normal file
View File

@@ -0,0 +1,9 @@
float64 base
float64 shoulder
float64 elbow
float64 roll
float64 wrist_pitch
float64 wrist_roll
bool clear_faults
bool reset_controllers

27
software/ros_packages/rover_arm/msg/ArmStatusMessage.msg Normal file
View File

@@ -0,0 +1,27 @@
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

6
software/ros_packages/rover_arm/package.xml
View File

@@ -51,10 +51,16 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>ros_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>ros_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>ros_msgs</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->

328
software/ros_packages/rover_arm/src/rover_arm.cpp
View File

@@ -1,11 +1,14 @@
#include <string>
#include <iostream>
#include <unistd.h>
#include <algorithm>
#include <ros/ros.h>
#include <ros/console.h>
#include "simplemotion/simplemotion.h"
#include <rover_arm/ArmControlMessage.h>
#include <rover_arm/ArmStatusMessage.h>
using namespace std;
@@ -14,21 +17,46 @@ int device_address = 1;
////////// Global Variables //////////
// ROS Parameter Defaults
const string default_port = "/dev/rover/ttyARM";
const string default_port = "/dev/ttyUSB0";
// Axis Defaults
const smint32 axis1_max_count = 100000;
float axis1_max_degrees = 180.0;
const string default_absolute_position_control_topic = "control/absolute";
const string default_relative_position_control_topic = "control/relative";
const string default_arm_status_topic = "status";
const smint32 axis2_max_count = 100000;
////////// Axis limits and conversions //////////
// Base
const smuint8 base_address = 1;
const smint32 base_counts_per_rev = 5725807;
const double base_min = -0.5;
const double base_max_rev = 0.5;
const smint32 axis3_max_count = 100000;
// Shoulder
const smuint8 shoulder_address = 2;
const smint32 shoulder_counts_per_rev = 5725807; //FIXME
const double shoulder_min_rev = -0.25;
const double shoulder_max_rev = 0.25;
const smint32 axis4_max_count = 100000;
//Elbow
const smuint8 elbow_address = 3;
const smint32 elbow_counts_per_rev = 5725807; //FIXME
const double elbow_min_rev = -0.5;
const double elbow_max_rev = 0.5;
const smint32 axis5_max_count = 320000;
//Roll
const smuint8 roll_address = 4;
const smint32 roll_counts_per_rev = 5725807; //FIXME
const double roll_min_rev = -0.25;
const double roll_max_rev = 0.25;
const smint32 axis6_max_count = 100000;
//Wrist Pitch
const smuint8 wrist_pitch_address = 5;
const smint32 wrist_pitch_counts_per_rev = 5725807; //FIXME
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
class RoverArm{
@@ -40,84 +68,274 @@ public:
node_handle->param("port", arm_port, default_port);
// Connect to arm, exit if failed
arm_bus_handle = smOpenBus(arm_port.c_str());
if(arm_bus_handle < 0){
ROS_ERROR("Could not connect to arm");
return;
}else{
arm_sucessfully_connected = true;
}
ROS_INFO("Connected to smBus");
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<rover_arm::ArmStatusMessage>(default_arm_status_topic, 1);
}
void run(){
char dir = 0;
smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 0);
smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, 0);
smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, 0);
while(ros::ok()){
smSetParameter(arm_bus_handle, device_address, SMP_FAULTS, 0);
smSetParameter(arm_bus_handle, device_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
smSetParameter(arm_bus_handle, 2, SMP_FAULTS, 0);
smSetParameter(arm_bus_handle, 2, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
smSetParameter(arm_bus_handle, 3, SMP_FAULTS, 0);
smSetParameter(arm_bus_handle, 3, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
if(!arm_sucessfully_connected){ return; }
smint32 status = 0;
smint32 status1 = 0;
smint32 status2 = 0;
smint32 position = 0;
smint32 position1 = 0;
smint32 position2 = 0;
smint32 read_bus_voltage;
while(!(status & STAT_TARGET_REACHED) || !(status1 & STAT_TARGET_REACHED) || !(status2 & STAT_TARGET_REACHED)){
smRead2Parameters(arm_bus_handle, device_address, SMP_STATUS, &status, SMP_ACTUAL_POSITION_FB, &position);
smRead2Parameters(arm_bus_handle, 2, SMP_STATUS, &status1, SMP_ACTUAL_POSITION_FB, &position1);
smRead2Parameters(arm_bus_handle, 3, SMP_STATUS, &status2, SMP_ACTUAL_POSITION_FB, &position2);
smRead1Parameter(arm_bus_handle, device_address, SMP_ACTUAL_BUS_VOLTAGE, &read_bus_voltage);
ROS_INFO("Dir: %u\tVoltage: %ld\tAxis 1: %ld\tAxis 2: %ld\tAxis 3: %ld", dir, read_bus_voltage, position, position1, position2);
}
dir = !dir;
if(dir){
smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, -50000);
smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, -50000);
smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, -50000);
}else{
smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 50000);
smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, 50000);
smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, 50000);
}
clear_faults();
get_joint_statuses();
set_joint_positions();
reset_controllers();
ros::spinOnce();
sleep(1);
}
smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 0);
smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, 0);
smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, 0);
ROS_ERROR("Shutting down.");
}
void reset_controllers(){
// Check if should reset
// Reset all, then kill thread
}
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);
should_clear_faults = false;
}
}
void get_joint_statuses(){
// Check if joint states valid
// SMP_STATUS
// SMP_FAULTS
// SMP_ACTUAL_POSITION
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);
}
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);
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;
// 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 );
//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_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){
set_base_position();
set_shoulder_position();
set_elbow_position();
set_roll_position();
set_wrist_positions();
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_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_elbow_position(){
}
void set_roll_position(){
}
void set_wrist_positions(){
}
void absolute_position_callback(const rover_arm::ArmControlMessage::ConstPtr& msg)
{
if(!received_first_joint_position_update){ return; }
// 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);
base_position = msg->base * base_counts_per_rev;
shoulder_position = msg->base * base_counts_per_rev;
// 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);
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);
}
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;
should_clear_faults += msg->clear_faults;
should_reset += msg->reset_controllers;
new_positions_received = true;
ROS_INFO("REL::::Base: %f\tshoulder: %f", msg->base, msg->shoulder);
}
private:
ros::NodeHandle *node_handle;
string arm_port;
smbus arm_bus_handle;
bool arm_sucessfully_connected = false;
ros::Publisher status_publisher;
rover_arm::ArmStatusMessage status_message;
ros::Subscriber absolute_position_control_subscriber;
ros::Subscriber relative_position_control_subscriber;
bool received_first_joint_position_update = false;
bool new_positions_received = false;
smint32 base_position = 0;
smint32 base_comm_state = 0;
smint32 base_status = 0;
smint32 base_faults = 0;
smint32 shoulder_position = 0;
smint32 shoulder_comm_state = 0;
smint32 shoulder_status = 0;
smint32 shoulder_faults = 0;
smint32 elbow_position = 0;
smint32 elbow_comm_state = 0;
smint32 elbow_status = 0;
smint32 elbow_faults = 0;
smint32 roll_position = 0;
smint32 roll_comm_state = 0;
smint32 roll_status = 0;
smint32 roll_faults = 0;
smint32 wrist_pitch_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_comm_state = 0;
smint32 wrist_roll_status = 0;
smint32 wrist_roll_faults = 0;
bool should_clear_faults = true;
bool should_reset = false;
};
int main(int argc, char** argv){
RoverArm rover_arm(argc, argv);
rover_arm.run();

4
software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt
View File

@@ -1,3 +1,3 @@
add_library(simplemotion rs232.c simplemotion.c busdevice.c sm_consts.c)
add_library(simplemotion2 rs232.c simplemotion.c busdevice.c sm_consts.c)
target_include_directories (simplemotion PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_include_directories (simplemotion2 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})