Created
August 18, 2019 22:20
-
-
Save droter/77108f3d12a21b577b90c588d7d09f8b to your computer and use it in GitHub Desktop.
Teensy software to test cytron motor controller and angle sensor from ROS cmd_vel
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <WProgram.h> | |
#include "ros.h" | |
#include "ros/time.h" | |
#include <math.h> | |
#include "geometry_msgs/Twist.h" | |
//header file for publishing velocities for odom | |
// Steer angle sensor | |
int steer_angle_val; | |
#define COMMAND_RATE 20 //hz | |
unsigned long g_prev_command_time = 0; | |
float linear_vel_x = 0; | |
float angular_vel_z = 0; | |
// Max steer motor value 255 | |
float steer_effort_max = 45; | |
int steer_center = 435; | |
int steer_left = 585; | |
int steer_right = 315; | |
//callback function prototypes | |
void commandCallback(const geometry_msgs::Twist& cmd_msg); | |
ros::NodeHandle nh; | |
ros::Subscriber<geometry_msgs::Twist> cmd_msg("cmd_vel", commandCallback); | |
geometry_msgs::Twist raw_vel_msg; | |
ros::Publisher raw_vel_pub("raw_vel", &raw_vel_msg); | |
float mapFloat(long x, long in_min, long in_max, long out_min, long out_max) | |
{ | |
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min; | |
} | |
void commandCallback(const geometry_msgs::Twist& cmd_msg) | |
{ | |
//callback function every time linear and angular speed is received from 'cmd_vel' topic | |
//this callback function receives cmd_msg object where linear and angular speed are stored | |
linear_vel_x = cmd_msg.linear.x; | |
angular_vel_z = cmd_msg.angular.z; | |
g_prev_command_time = millis(); | |
} | |
int steer_Ki_count = 0; | |
int steer_Ki_timeout = 10; | |
int steer_Ki = 10; | |
int steer_last_value = 0; | |
double steer_err_value = 0; | |
void steer() | |
{ | |
//nh.loginfo("In steer function"); | |
//steering function for ACKERMANN base | |
// From TEB Planner for Carelike vehicles | |
// wheelbase = 0.8509 | |
/* | |
def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase): | |
if omega == 0 or v == 0: | |
return 0 | |
radius = v / omega | |
return math.atan(wheelbase / radius) | |
// v = data.linear.x | |
// steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase) | |
*/ | |
//this converts angular velocity(rad) to steering angle(degree) | |
float steering_angle_target; | |
float steering_angle_deg; | |
//convert steering angle from rad to deg | |
steering_angle_deg = angular_vel_z * (180 / PI); | |
if(steering_angle_deg > 0) | |
{ | |
//steer left | |
steering_angle_target = mapFloat(steering_angle_deg, 0, 60, steer_center, steer_right); | |
} | |
else if(steering_angle_deg < 0) | |
{ | |
//steer right | |
steering_angle_target = mapFloat(steering_angle_deg, 0, -60, steer_center, steer_left); | |
} | |
else | |
{ | |
//return steering wheel to middle if there's no command | |
steering_angle_target = steer_center; | |
} | |
char buffer[50]; | |
sprintf (buffer, "Steer cmd_vel : %f", angular_vel_z); | |
nh.loginfo(buffer); | |
sprintf (buffer, "Steer angle value : %f", steering_angle_target); | |
nh.loginfo(buffer); | |
steer_angle_val = analogRead(15); | |
steer_err_value = steering_angle_target - steer_angle_val; | |
sprintf (buffer, "Steer sensor Value : %d", steer_angle_val); | |
nh.loginfo(buffer); | |
// Need PID to calc this value | |
int steer_effort = (steer_err_value * steer_Ki) + steer_last_value; | |
steer_last_value = steer_effort; | |
steer_Ki_count++; | |
if (steer_Ki_count > steer_Ki_timeout){ | |
steer_Ki_count = 0; | |
steer_last_value = 0; | |
} | |
// Max steer motor value 255 | |
if (steer_effort > steer_effort_max){ | |
steer_effort = steer_effort_max; | |
} else if ((steer_effort < 0) && (steer_effort > -steer_effort_max)){ | |
steer_effort = abs(steer_effort); | |
} else if (steer_effort < -steer_effort_max){ | |
steer_effort = steer_effort_max; | |
} | |
// set the direction of motor | |
if (steer_err_value < -20){ | |
// nh.loginfo("steer_left"); | |
digitalWrite(14, HIGH); | |
analogWrite(20, abs(steer_effort)); | |
} else if (steer_err_value > 20){ | |
// nh.loginfo("steer_right"); | |
digitalWrite(14, LOW); | |
analogWrite(20, abs(steer_effort)); | |
} else { | |
analogWrite(20, 0); | |
} | |
} | |
void stopBase() | |
{ | |
linear_vel_x = 0; | |
angular_vel_z = 0; | |
} | |
void moveBase() | |
{ | |
//velocityControl(); | |
steer(); | |
} | |
void setup() { | |
nh.initNode(); | |
nh.getHardware()->setBaud(57600); | |
nh.subscribe(cmd_msg); | |
nh.advertise(raw_vel_pub); | |
// Steer angle sensor | |
pinMode(15, INPUT); | |
// Steer motor | |
pinMode(14, OUTPUT); | |
pinMode(20, OUTPUT); | |
while (!nh.connected()) | |
{ | |
nh.spinOnce(); | |
} | |
nh.loginfo("Tractor is connected"); | |
delay(1); | |
} | |
void loop() { | |
// Limit the rate you publish | |
static unsigned long prev_control_time = 0; | |
//this block drives the robot based on defined rate | |
if ((millis() - prev_control_time) >= (1000 / COMMAND_RATE)) | |
{ | |
moveBase(); | |
prev_control_time = millis(); | |
} | |
//this block stops the motor when no command is received | |
if ((millis() - g_prev_command_time) >= 400) | |
{ | |
stopBase(); | |
} | |
// reverseState = digitalRead(reverseSwitch); // 1 is forward, 0 is reverse | |
//call all the callbacks waiting to be called | |
nh.spinOnce(); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Using rosserial_python http://wiki.ros.org/rosserial_python
Connect the teensy to your ROS computer and run this command:
rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=57600