ROS code to subscribe to cmd_vel and sends commands to motor controller that controls steering for lawn tractor. Currently using Teensy as microcontroller.

steer_motor_v3.cpp

/*

steer_motor_v3.cpp

Subscribes to cmd_vel 
Sends commands to motor controller connected to Teensy using commands digitalWrite(DIRPin, HIGH); 

*/
#include "ros.h"
#include "std_msgs/UInt16.h"
#include "std_msgs/Int16.h"
#include "std_msgs/Int32.h"
#include "geometry_msgs/Twist.h"
ros::NodeHandle nh;

int next_pos = 0;
int steer_angle_val = 0;
int steer_err_value = 0;
int steer_last_value = 0;
int steer_effort = 60;  // max_steer_eff
int steer_effort_b4 = 0;
int steer_Ki_count = 0;
int steer_Ki_timeout = 10;
int steer_Ki = 3;
float steering_angle_target = 0;
float steering_angle_deg = 0;

int DIRPin = 14; 
int PWMPin = 20;
char buffer[250];

const int steerInterval = 20;  // 100 10 HZ, 50 20Hz, 20 = 50 Hz
const int infoInterval = 3000;  // 100 = 1/10 of a second (i.e. 10 Hz) 5000 = 5 seconds
const int infoIntervalSteer = 3000; 
const int paramsInterval = 10000;
unsigned long prev_cmd_vel_time = 0;
unsigned long prev_time_stamp_steer = 0;
unsigned long prev_time_stamp_info = 0;
unsigned long prev_time_stamp_infoSteer = 0;
unsigned long prev_time_stamp_params = 0;

float angular_vel_z = 0;

float steer_settings[5];

// steering settings
int steer_left_max = -14000;
int steer_right_max = 13500;
int steer_straight = 0;
int max_steer_eff = 50;
//const int max_steer_eff_neg = max_steer_eff * -1;
const int steer_right_buffer = 200;
const int steer_left_buffer = -200;
const float cmd_vel_full_left = .85;
const float cmd_vel_full_right = -.76;

void getFrontAngle(const std_msgs::Int32& front_angle_raw) {
  steer_angle_val = front_angle_raw.data;
}

void getCMD_VEL(const geometry_msgs::Twist& cmd_msg) {
    angular_vel_z = cmd_msg.angular.z;
    prev_cmd_vel_time = millis();
}

ros::Subscriber<std_msgs::Int32> front_angle_raw("/front_angle_raw", &getFrontAngle);

//ros::Subscriber<geometry_msgs::Twist> cmd_msg("/teleop/cmd_vel", &getCMD_VEL);
ros::Subscriber<geometry_msgs::Twist> cmd_msg("/cmd_vel", &getCMD_VEL);
//std_msgs::UInt16 front_angle_target;
std_msgs::Int16 front_angle_target;  
ros::Publisher steer_angle_target("/front_angle_target", &front_angle_target);

std_msgs::UInt16 steer_effort_raw;  
ros::Publisher steer_effort_raw_pub("/steer_effort_raw", &steer_effort_raw);
std_msgs::UInt16 steer_effort_b4_raw;  
ros::Publisher steer_effort_b4_raw_pub("/steer_effort_b4_raw", &steer_effort_b4_raw);

float mapfloat(float x, float in_min, float in_max, float out_min, float out_max){
  return (x - in_min)*(out_max - out_min) / (in_max - in_min) + out_min;
}

void steerBase() {
    // convert angular.z to target range
    // Scale the cmd_vel input to full left and full right of mower
    if (angular_vel_z < 0)
      {
      if (angular_vel_z < cmd_vel_full_right) {angular_vel_z = cmd_vel_full_right;}
      steering_angle_target = mapfloat(angular_vel_z, 0, cmd_vel_full_right, 0, steer_right_max);
      }
    else if (angular_vel_z > 0)
        {
        if (angular_vel_z > cmd_vel_full_left) {angular_vel_z = cmd_vel_full_left;}
        steering_angle_target = mapfloat(angular_vel_z, 0, cmd_vel_full_left, 0, steer_left_max);
        }
    else {steering_angle_target = steer_straight;}

    // publish steer angle target
    front_angle_target.data = steering_angle_target;
    steer_angle_target.publish(&front_angle_target);

    steer_err_value = steering_angle_target - steer_angle_val;   
    steer_effort = (steer_err_value * steer_Ki) + steer_last_value;
    steer_last_value = steer_effort;

    // Steer I
    steer_Ki_count++;
    if (steer_Ki_count > steer_Ki_timeout){
      steer_Ki_count = 0;
      steer_last_value = 0;
    } 

    // publish steer effort
    steer_effort_b4 = steer_effort;   // only used for reporting/debugging
    steer_effort_b4_raw.data = steer_effort;
    steer_effort_b4_raw_pub.publish(&steer_effort_b4_raw); 

    if (steer_effort > max_steer_eff){  // need to check if the PID can actually produce a negative steer_effort
      steer_effort = max_steer_eff;
    } else if ((steer_effort < 0) && (steer_effort > -max_steer_eff)){
      steer_effort = abs(steer_effort);
    } else if (steer_effort < -max_steer_eff){
      steer_effort = max_steer_eff;
    }

    // publish steer effort
    steer_effort_raw.data = steer_effort;
    steer_effort_raw_pub.publish(&steer_effort_raw);  

    // Set the direction of motor and drive the motor.  Only adjust the steer angle if error is greater than buffer values
    // Adjust higher or lower depending on how active the steering is (jitter)
    // the pid output is -255 to 255.  I need to adjust the pid to see  where it is at at angular.z +/- .01 and .02
    if (steer_err_value < steer_left_buffer){
      digitalWrite(14, HIGH);
      analogWrite(20, abs(steer_effort)); 
    } else if (steer_err_value > steer_right_buffer){
      digitalWrite(14, LOW);
      analogWrite(20, abs(steer_effort)); 
    } else {
      analogWrite(20, 0); 
    }

  if (millis() - prev_time_stamp_infoSteer >= infoIntervalSteer) {  // provide an informational message
    sprintf (buffer, "steer motor - raw cmd_vel_angular_z :%f steering_angle_target:%f current angle: %d steer_err_value: %d steer_effort_b4: %d steer_effort: %d", 
      angular_vel_z, steering_angle_target, steer_angle_val, steer_err_value, steer_effort_b4, steer_effort);

    nh.loginfo(buffer);    
    prev_time_stamp_infoSteer = millis();
  }  
}

void stopBase() {
  angular_vel_z = 0;
}

void setup() {
  pinMode(DIRPin, OUTPUT);
  nh.initNode();
  nh.subscribe(front_angle_raw);
  nh.subscribe(cmd_msg);
  nh.advertise(steer_angle_target);
  nh.advertise(steer_effort_raw_pub);
  nh.advertise(steer_effort_b4_raw_pub);  

  while (!nh.connected()){
    nh.spinOnce();
  }
  nh.loginfo("steer control - Tractor is connected (last compiled 20220227 ~10:05 pm");

/*
  while ((steer_angle_val < steer_left_max)) { // check to make sure the angle sensor is publishing
    nh.spinOnce();
    sprintf (buffer, "steer control - setup - waiting for valid front_angle_klm: %d", steer_angle_val);
    nh.loginfo(buffer);
    delay(2000);
  }
  */
  nh.getParam("/steering", steer_settings,5);
  delay(100);
  sprintf (buffer, "steer control - setup - initial front_angle_klm: %d", steer_angle_val);
  nh.loginfo(buffer);
  delay(1000);
}

void loop() {
  if ((millis() - prev_time_stamp_steer) >= steerInterval) {
    steerBase();
    prev_time_stamp_steer = millis();
  }
  if ((millis() - prev_cmd_vel_time) >= 400) {    //this steers straight when no command is received
    stopBase();
  }
  if (millis() - prev_time_stamp_info >= infoInterval) {  // provide an informational message
    sprintf (buffer, "steer cmd_vel - raw cmd_vel_angular_z :%f steering_angle_target:%f current angle %d steer_err_value %d", angular_vel_z, steering_angle_target, steer_angle_val, steer_err_value);
    nh.loginfo(buffer);
    prev_time_stamp_info = millis();
  }  
  if (millis() - prev_time_stamp_params >= paramsInterval) {  // retrieve paramaters
    nh.getParam("/steering", steer_settings,5);
    steer_left_max = steer_settings[0];
    steer_right_max = steer_settings[1];
    steer_straight = steer_settings[2];
    max_steer_eff = steer_settings[3];
    steer_Ki = steer_settings[4];
 
    sprintf (buffer, "steer control - param retrieval - max_steer_eff :%d steer_straight: %d", max_steer_eff, steer_straight);
    nh.loginfo(buffer);
    prev_time_stamp_params = millis();
  }    
  nh.spinOnce();
}