This I used to test the TEENSY. With this script, the led blinks. It is for TEENSY 3.1 and 3.2

main.cpp

#include <WProgram.h>
#include "ros.h"
#include "ros/time.h"
#include <Servo.h>
#include <math.h>
#include <TimerOne.h>
#include "std_msgs/UInt8.h"
#include "std_msgs/Int64.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32.h"
#include <ros/service_client.h>
#include "lawn_tractor/relayCmd.h"


//header file for cmd_subscribing to "cmd_vel"
#include "geometry_msgs/Twist.h"
//header file for publishing velocities for odom
// #include "lino_msgs/Velocities.h"

// TODO:  Startup routine to set motor position 
//        Set motor direction based on sign
//        
//       Getting strange tick results.   


Servo myservo;
float linear_vel_x = 0;
float angular_vel_z = 0;

// Steer angle sensor
int steer_angle_val;

// Reverse Switch
int reverseSwitch = 5;
int reverseState = 0;

// Encoder Ticks
volatile byte ticks;
volatile byte interrupt = 0;
int encoder_tick_count = 0;
// static unsigned long last_encoder_time = 0;

// Tractor State Switch
int startServoPower = 0;

#define COMMAND_RATE 20 //hz
unsigned long g_prev_command_time = 0;

//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);

//std_msgs::UInt8 teensy_state_msg;
//ros::Publisher teensy_state_pub("teensy_state", &teensy_state_msg);

// Relay Service call setup
// ros::ServiceClient relay_client = nh.serviceClient<lawn_tractor::relayCmd>("/relay_cmd");
ros::ServiceClient<lawn_tractor::relayCmd::Request, lawn_tractor::relayCmd::Response> client("/relay_cmd");
// lawn_tractor::relayCmd::Request request;
// lawn_tractor::relayCmd::Response response;
// nh.serviceClient(relay_client);
using lawn_tractor::relayCmd;




void tick_detect()
{
  ticks++;
  //interrupt = 1;
}

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


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 odom(steering_angle)
// {}


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, 325, 450);
    }
    else if(steering_angle_deg < 0)
    {
        //steer right
        steering_angle_target = mapFloat(steering_angle_deg, 0, -60, 325, 195);
    }
    else
    {
        //return steering wheel to middle if there's no command
        steering_angle_target = 325;
    }

    char buffer[50];
    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 error : %f", steer_err_value);
    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 > 55){
      steer_effort = 55;
    } else if ((steer_effort < 0) && (steer_effort > -55)){
      steer_effort = abs(steer_effort);
    } else if (steer_effort < -55){
      steer_effort = 55;
    }

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

    // call to odom
    //odom(steering_angle);
}


void zero_velocity(){
  do {
    digitalWrite(22, HIGH);
    digitalWrite(23, LOW);
    analogWrite(21, 200); 
    } while (!reverseState);
    digitalWrite(22, LOW);
    digitalWrite(23, HIGH);
    analogWrite(21, 200);
    delay(1);
    analogWrite(21, 0); 
}




int Ki_count = 0;
int Ki_timeout = 20;
int Ki = 20;
int last_value = 0;
double err_value = 0;

void velocityControl(){
  err_value = raw_vel_msg.linear.x - linear_vel_x;
     
  // Need PID to calc this value
  int vel_effort = (err_value * Ki) + last_value;
  last_value = vel_effort;

  Ki_count++;
  if (Ki_count > Ki_timeout){
    Ki_count = 0;
    last_value = 0;
  } 

  // Max motor value 255
  if (vel_effort > 255){
    vel_effort = 255;
  } else if ((vel_effort < 0) && (vel_effort > -255)){
    vel_effort = abs(vel_effort);
  } else if (vel_effort < -255){
    vel_effort = 255;
  }

  // set the direction of motor
  if (err_value < -0.05){
    // nh.loginfo("move forward");
    digitalWrite(22, LOW);
    digitalWrite(23, HIGH);
    // analogWrite(21, 0); 
    analogWrite(21, abs(vel_effort)); 
  } else if (err_value > .05){
    // nh.loginfo("move backward");
    digitalWrite(22, HIGH);
    digitalWrite(23, LOW);
    //analogWrite(21, 0); 
    analogWrite(21, abs(vel_effort)); 
  } else {
    analogWrite(21, 0); 
    //zero_velocity(); 
  }

  // char buffer[50];
  // sprintf (buffer, "Current Velocity Motor  : %d", vel_effort);
  // nh.loginfo(buffer);

}


void stopBase()
{
  linear_vel_x = 0;
  angular_vel_z = 0;
}

void moveBase()
{
  //  need to publish encoder as Twist
  //  Run calculated velocity through smoother
  //  Read smoothed velocity or output of odom
  //  Adjust speed

  //  current speed - desired speed
  //  send motor control
  velocityControl();
  steer();
}

const int filterWeight = 16;  //higher number heavier filter
const int numReadings = 10;

double average_speed = 0;

void timerIsr()
{
  Timer1.detachInterrupt();  //stop the timer
  // right_wheel_vel.data = float(counter_right)*2*pi*5/8;
  // right_wheel_vel_pub.publish(&right_wheel_vel);
  // counter_right=0;

  double speed = ((ticks*(2*3.1415*0.2413)/30)/.2);

  reverseState = digitalRead(reverseSwitch); // 1 is forward, 0 is reverse
  if (!reverseState){
    nh.loginfo("In REVERSE");
    speed = speed * -1;
  }

  for (int i = 0; i < numReadings; i++) {
    average_speed = average_speed + (speed - average_speed) / filterWeight;
  }

  // publish raw speed
  raw_vel_msg.linear.x = average_speed;
  raw_vel_msg.linear.y = 0;
  raw_vel_msg.linear.z = 0;
  raw_vel_msg.angular.x = 0;
  raw_vel_msg.angular.y = 0;
  raw_vel_msg.angular.z = 0;
  raw_vel_pub.publish(&raw_vel_msg);

  //char buffer[50];
  //sprintf (buffer, "Speed  : %d", ticks );
  //nh.loginfo(buffer);

  ticks = 0;
  Timer1.attachInterrupt( timerIsr );  //enable the timer
}


bool set_relay(const lawn_tractor::relayCmd::Request& relay_cmd)
{
    relayCmd::Request relay_request;
    relayCmd::Response response;
    relay_request.channel = relay_cmd.channel;
    relay_request.state = relay_cmd.state;

    response.success = false;

    client.call(relay_request, response);
    delay(1000);

    if(response.success){
      return true;
    } else {
      return false; 
    }
}


void setup() {
  nh.initNode();
  nh.getHardware()->setBaud(57600);
  nh.subscribe(cmd_msg);
  nh.advertise(raw_vel_pub);
  // nh.advertise(teensy_state_pub);
  nh.serviceClient(client);

  // setup for velocity motor
  pinMode(21, OUTPUT);
  pinMode(22, OUTPUT);
  pinMode(23, OUTPUT);

  // setup servo pin
  // myservo.attach(20);
  // myservo.write(90);

  // Servo pin is active start motor
  startServoPower = 0; 

  // Steer angle sensor
  pinMode(15, INPUT);
  // Steer motor
  pinMode(14, OUTPUT);
  pinMode(20, OUTPUT);

  // setup reverse switch
  pinMode(reverseSwitch, INPUT_PULLUP);

  // wheel encoder
  Timer1.initialize(200000);
  attachInterrupt(6, tick_detect, FALLING);
  ticks = 0;
  Timer1.attachInterrupt( timerIsr ); // enable the timer  

  // Zero motor
  // zero_velocity(); 


  while (!nh.connected())
  {
      nh.spinOnce();
  }
  nh.loginfo("Tractor is connected");
  delay(1);
}


void loop() {

  // Limit the rate you publish
  // Not sure how to do this
  if (startServoPower){
    relayCmd::Request startServo;
    startServo.channel = 2;
    startServo.state = 1; 
    set_relay(startServo);

    startServoPower = 0;
    delay(100);
  }

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