Teensy software to test cytron motor controller and angle sensor from ROS cmd_vel

steer motor test

#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();
}

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