Skip to content

Commit

Permalink
latest changes for quad
Browse files Browse the repository at this point in the history
  • Loading branch information
@mstuettgen
mstuettgen committed Jun 25, 2015
1 parent ac32cd4 commit 4db20e9
Show file tree
Hide file tree
Showing 6 changed files with 372 additions and 303 deletions.
4 changes: 2 additions & 2 deletions rover_nodes/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,7 @@ include_directories(

## Declare a cpp executable
add_executable(actuator_control_publisher src/actuator_control_publisher.cpp)
add_executable(pixhawk_teleop src/pixhawk_teleop.cpp)
add_executable(teleop src/teleop.cpp)
add_executable(cmd_vel_to_actuator src/cmd_vel_to_actuator.cpp)
add_executable(cmd_vel_to_can src/cmd_vel_to_can.cpp)

Expand All @@ -119,7 +119,7 @@ add_dependencies(actuator_control_publisher mavros_generate_messages_cpp)
target_link_libraries(actuator_control_publisher
${catkin_LIBRARIES}
)
target_link_libraries(pixhawk_teleop
target_link_libraries(teleop
${catkin_LIBRARIES}
)
target_link_libraries(cmd_vel_to_actuator
Expand Down
6 changes: 0 additions & 6 deletions rover_nodes/launch/pixhawk_teleop.launch
View file

This file was deleted.

31 changes: 0 additions & 31 deletions rover_nodes/launch/pixhawk_teleop.yaml
View file

This file was deleted.

226 changes: 176 additions & 50 deletions rover_nodes/src/cmd_vel_to_can.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,59 +13,162 @@
#include <fcntl.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>

#define CAN_BAUD_1M 0x0014 //1 Mbit/s
#define CAN_BAUD_500K 0x001C // 500 kBit/s
#define CAN_BAUD_250K 0x011C // 250 kBit/s
#define CAN_BAUD_125K 0x031C // 125 kBit/s
#define CAN_BAUD_100K 0x432F // 100 kBit/s
#define CAN_BAUD_50K 0x472F // 50 kBit/s
#define CAN_BAUD_20K 0x532F // 20 kBit/s
#define CAN_BAUD_10K 0x672F // 10 kBit/s
#define CAN_BAUD_5K 0x7F7F //5 kBit/s

#define MAX_STEERING_ANGLE 25
#define THROTTLE_ZERO 2000
#define THROTTLE_MAX 3600


class CmdVelToCAN {
public:
void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd)
{
throttle_ = vel_cmd.linear.x;
yaw_ = -vel_cmd.angular.z;
//ROS_INFO("CMD_VEL MESSAGE RECEIVED");

yaw_ = -vel_cmd.angular.z * (float)MAX_STEERING_ANGLE;

//project throttle from [-1:1] to [THROTTLE_ZERO:THROTTLE_MAX]
double m = (THROTTLE_MAX - THROTTLE_ZERO) / 1.0;
double x = vel_cmd.linear.x;

if (x >= 0.05) {
forward=true;
reverse=false;
}

if(x < -0.05) {
forward=false;
reverse=true;
x = (-1.0) * x; //don't send negative values to quad motor!
}

double b = THROTTLE_ZERO;
throttle_ = m * x + b ;
}

void _publish()
{
ROS_INFO("_publish()");
printf("yaw: %f\n", yaw_);
printf("throttle: %f\n" , throttle_);
yaw_IEEE754 = pack754(yaw_, 32, 8);
yaw_IEEE754_2 = yaw_IEEE754;
std::cout << "yaw ieee754: " << "0x" <<std::hex << yaw_IEEE754 << std::endl;
std::cout << "yaw ieee754_2: " << "0x" <<std::hex << yaw_IEEE754_2 << std::endl;
std::cout << "sizeof(yawyaw_IEEE754_2): " << sizeof(yaw_IEEE754_2) << std::endl;
int a = (yaw_IEEE754_2 >> (8*0)) & 0xff;
int b = (yaw_IEEE754_2 >> (8*1)) & 0xff;
int c = (yaw_IEEE754_2 >> (8*2)) & 0xff;
int d = (yaw_IEEE754_2 >> (8*3)) & 0xff;
std::cout << "int a: " << std::hex << a << std::endl;
std::cout << "int b: " << std::hex << b << std::endl;
std::cout << "int c: " << std::hex << c << std::endl;
std::cout << "int d: " << std::hex << d << std::endl;
throttle_IEEE754 = pack754(throttle_, 32 , 8 );
// std::cout << "yaw ieee754: " << "0x" <<std::hex << yaw_IEEE754 << std::endl;
// std::cout << "throttle ieee754: " << "0x" <<std::hex << throttle_IEEE754 << std::endl;
// std::cout << "zero ieee754: " << "0x" <<std::hex << zero_IEEE754 << std::endl;
// std::cout << "one ieee754: " << "0x" <<std::hex << one_IEEE754 << std::endl;

std::cout << "forward: " << forward << std::endl;
std::cout << "reverse: " << reverse << std::endl;
std::cout << "reverse_on: " << reverse_on << std::endl;


if(forward) {
if(reverse_on) {
ROS_WARN_NAMED("test","writing forward throttle msg");
if(!CAN_Write(h_, &normal_throttle_msg_)) {
ROS_INFO("Success");
reverse_on=false;
usleep(250000);
}
else
ROS_ERROR("ERROR");
}
}

if(reverse) {
if(!reverse_on) {
ROS_ERROR("writing reverse throttle msg...\n");
if(!CAN_Write(h_, &reverse_throttle_msg_)) {
ROS_INFO("Success");
reverse_on=true;
usleep(250000);
}
else
ROS_ERROR("ERROR");
}
}

//steering msg
canmsg_.ID = 2;
canmsg_.MSGTYPE = MSGTYPE_STANDARD;
canmsg_.LEN = sizeof(yaw_IEEE754_2);
for(int i=0 ; i<canmsg_.LEN; i++ )
can_steering_msg_.ID = 2;
can_steering_msg_.MSGTYPE = MSGTYPE_STANDARD;
can_steering_msg_.LEN = sizeof(yaw_IEEE754);
for(int i=0 ; i<can_steering_msg_.LEN; i++ )
{
canmsg_.DATA[i] = (yaw_IEEE754_2 >> (8*(canmsg_.LEN-(i+1)) )) & 0xff;
can_steering_msg_.DATA[i] = (yaw_IEEE754 >> (8*i)) & 0xff;
}

//throttle msg
can_throttle_msg_.ID = 1;
can_throttle_msg_.MSGTYPE = MSGTYPE_STANDARD;
can_throttle_msg_.LEN = sizeof(throttle_IEEE754);
for(int i=0 ; i<can_throttle_msg_.LEN; i++ )
{
can_throttle_msg_.DATA[i] = (throttle_IEEE754 >> (8*i)) & 0xff;
}

CAN_Write(h_, &canmsg_);

//clear CAN status
CAN_Status(h_);

std::cout << "writing steering msg..." << std::endl;

if(!CAN_Write(h_, &can_steering_msg_))
ROS_INFO("Success");
else
ROS_ERROR("ERROR");

std::cout << "writing throttle msg..." << std::endl;

if(!CAN_Write(h_, &can_throttle_msg_))
ROS_INFO("Success");
else
ROS_ERROR("ERROR");

std::cout << "i'm finished" << std::endl;

}

//constructor
CmdVelToCAN() :
yaw_(20.0),
throttle_(0.0)
yaw_(0.0),
throttle_(2000.0),
reverse_on(false),
forward(true),
reverse(false)
{
nh_.param<std::string>("cmd_vel_topic", cmd_vel_topic_, "/cmd_vel");
nh_.param<std::string>("can_device", can_device_, "/dev/pcanusb3");
nh_.param<std::string>("can_device", can_device_, "/dev/pcanusb0");

cmd_vel_sub_ = nh_.subscribe(cmd_vel_topic_, 1, &CmdVelToCAN::cmd_vel_callback, this);

errno=0;

char baudrate[7] = "0x0014"; //1 Mbit/s
//char baudrate[7] = "0x001C"; //500 kbit/s
//char baudrate[7] = "0x011C"; //250 kbit/s

std::cout << "here scheppert dat" << std::endl;
int wBTR0BTR1 = (int)strtoul(baudrate, NULL, 16);
int nExtended = 0;

std::cout << wBTR0BTR1 << std::endl;
std::cout << nExtended << std::endl;

h_ = LINUX_CAN_Open(can_device_.c_str(), O_RDWR);
if (!h_) {
ROS_ERROR("CMD_VEL_TO_CAN: can't open %s\n", can_device_.c_str());
return;
}
else {
ROS_INFO("CMD_VEL_TO_CAN: succesfully opened %s\n", can_device_.c_str());
Expand All @@ -74,10 +177,24 @@ class CmdVelToCAN {
/* clear status */
CAN_Status(h_);

errno = CAN_Init(h_, wBTR0BTR1, 0);
std::cout << "bla bla" << std::endl;

//prepare reverse / normal throttle msgs
one_IEEE754 = 1;
zero_IEEE754 = 0;

reverse_throttle_msg_.ID = 3;
reverse_throttle_msg_.MSGTYPE = MSGTYPE_STANDARD;
reverse_throttle_msg_.LEN = 1;
reverse_throttle_msg_.DATA[0] = one_IEEE754;

normal_throttle_msg_.ID = 3;
normal_throttle_msg_.MSGTYPE = MSGTYPE_STANDARD;
normal_throttle_msg_.LEN = 1;
normal_throttle_msg_.DATA[0] = zero_IEEE754;

ros::Rate r(10);
ros::Rate r(20);

while(ros::ok()) {
_publish();
Expand All @@ -95,41 +212,50 @@ class CmdVelToCAN {
std::string can_device_;
float yaw_, throttle_;
uint64_t yaw_IEEE754;
uint yaw_IEEE754_2;
uint64_t throttle_IEEE754;
uint64_t one_IEEE754;
uint64_t zero_IEEE754;
bool forward, reverse;
bool reverse_on;

//CAN specific stuff (from PEAK Systems SDK)
int nType_ = HW_USB;
HANDLE h_;
TPCANMsg canmsg_;
TPCANMsg can_steering_msg_;
TPCANMsg can_throttle_msg_;
TPCANMsg reverse_throttle_msg_;
TPCANMsg normal_throttle_msg_;


uint64_t pack754(long double f, unsigned bits, unsigned expbits)
{
long double fnorm;
int shift;
long long sign, exp, significand;
unsigned significandbits = bits - expbits - 1; // -1 for sign bit
//converst double value to IEEE754 float
uint64_t pack754(long double f, unsigned bits, unsigned expbits)
{
long double fnorm;
int shift;
long long sign, exp, significand;
unsigned significandbits = bits - expbits - 1; // -1 for sign bit

if (f == 0.0) return 0; // get this special case out of the way
if (f == 0.0) return 0; // get this special case out of the way

// check sign and begin normalization
if (f < 0) { sign = 1; fnorm = -f; }
else { sign = 0; fnorm = f; }
// check sign and begin normalization
if (f < 0) { sign = 1; fnorm = -f; }
else { sign = 0; fnorm = f; }

// get the normalized form of f and track the exponent
shift = 0;
while(fnorm >= 2.0) { fnorm /= 2.0; shift++; }
while(fnorm < 1.0) { fnorm *= 2.0; shift--; }
fnorm = fnorm - 1.0;
// get the normalized form of f and track the exponent
shift = 0;
while(fnorm >= 2.0) { fnorm /= 2.0; shift++; }
while(fnorm < 1.0) { fnorm *= 2.0; shift--; }
fnorm = fnorm - 1.0;

// calculate the binary form (non-float) of the significand data
significand = fnorm * ((1LL<<significandbits) + 0.5f);
// calculate the binary form (non-float) of the significand data
significand = fnorm * ((1LL<<significandbits) + 0.5f);

// get the biased exponent
exp = shift + ((1<<(expbits-1)) - 1); // shift + bias
// get the biased exponent
exp = shift + ((1<<(expbits-1)) - 1); // shift + bias

// return the final answer
return (sign<<(bits-1)) | (exp<<(bits-expbits-1)) | significand;
}
// return the final answer
return (sign<<(bits-1)) | (exp<<(bits-expbits-1)) | significand;
}

};

Expand All @@ -138,7 +264,7 @@ uint64_t pack754(long double f, unsigned bits, unsigned expbits)
int main(int argc, char *argv[])
{
//Initiate ROS
ros::init(argc, argv, "can_teleop");
ros::init(argc, argv, "cmd_vel_to_can");
//Create an object of class that will take care of everything
CmdVelToCAN p;
while(ros::ok()) {
Expand Down
Loading

0 comments on commit 4db20e9

Please sign in to comment.