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

Relative and absoute work. Cal starting to work. Moving to rover for controller integration.

Browse Source
This commit is contained in:
Corwin Perren
2018-08-04 18:51:24 -07:00
parent 7f80f44b96
commit ddd4d6b60a
3 changed files with 225 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
software/ros_packages/rover_arm/msg/ArmControlMessage.msg
View File

@@ -6,4 +6,5 @@ float64 wrist_pitch
float64 wrist_roll
bool clear_faults
bool reset_controllers
bool reset_controllers
bool calibrate

12
software/ros_packages/rover_arm/msg/ArmStatusMessage.msg
View File

@@ -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

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

@@ -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<rover_arm::ArmStatusMessage>(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();
}