Super Servo ROS enabled and control the transmission of a lawn tractor

transmission_v3.cpp

/*

This program is designed to run on a Teensy that is connected to a SuperServo to make the Super Servo ROS enabled and control 
the transmission of a lawn tractor.  It is started in a launch file as one of multiple nodes with the command: 
<node ns="transmission_teensy" name="transmission_cntrl" pkg="rosserial_python" type="serial_node.py" args="/dev/transmission_control" output="screen" />

Overview:
    subscribe to a target speed in cmd_vel format (i.e. cmd_vel.linear.x)
    subscribe to actual speed from the rear wheels 
    adjust servo position to match actual speed to target speed
        writes PWM value to Pin 20
        the frequency set in VelocityInterval

Prerequisits include udev rules pointing to /dev/transmission_control

Enhancements:
10/31/21 - Established pre-set servo settings for speed control

*/
#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <Servo.h>
#include "std_msgs/UInt16.h"
#include "std_msgs/Float32.h"
//#include <iostream>
//using namespace std;
Servo Transmission_Servo;

char buffer[180];
float speed_target;
const int infoInterval = 1000;  // 100 = 1/10 of a second (i.e. 10 Hz)
const int VelocityInterval = 100;  // 100 = 1/10 of a second (i.e. 10 Hz)
const int failsafe_interval = 500;  // 500 = 1/2 of a second (i.e. 2 Hz)
const int transmission_PWM_pub_interval = 100;
const int paramsInterval = 15000;
float currentTime = 0;
float previousTime = 0;
float elapsedTime = 0;
float prev_transmission_PWM_time = 0;
//float prev_time_stamp_info =  0;
unsigned long prev_time_stamp_info =  0;
//float prev_time_stamp_velocity =  0;
unsigned long prev_time_stamp_velocity =  0;
// float prev_cmd_vel_time = 0;
unsigned long prev_cmd_vel_time = 0;
float time_delta =0;
// float prev_time_stamp_params = 0;
unsigned long prev_time_stamp_params = 0;
//float last_left_speed_time = 0;
int vel_eff_target = 0;
int vel_start = 0;
int vel_effort = 0; 

//float start_forward_nudge = 125;   // 125 for flat concrete
//float start_reverse_nudge = -100;  // untested
//int pwm_value = 0;
float left_speed_avg_val = 0;
float speed_avg_val = 0;


//float speed_settings[9];
/*
 * For safety I will comment out the array that is near what I think will be production values and opt for low values for safety.  I can adjust using the params settings for
 * testing and then come back and update the production version with the updated values.
 */
//float speed_settings[9] = {1000.0, 1100.0, 1290.0, 1300.0, 1350.0, 1490.0, 1525.0, 1.0, 2.0};
float speed_settings[9] = {1100.0, 1100.0, 1290.0, 1300.0, 1300.0, 1300.0, 1300.0, 1.0, 2.0};
float full_reverse = speed_settings[0];
float first_reverse = speed_settings[1];
float vel_neutral = speed_settings[2];
float first_forward = speed_settings[3];
float second_forward = speed_settings[4];
float third_forward = speed_settings[5]; 
float full_forward = speed_settings[6];

ros::NodeHandle  nh;  

float mapDouble(double x, double in_min, double in_max, double out_min, double out_max){
    return (float)(((x - in_min) * (out_max - out_min)) / (float)(in_max - in_min)) + out_min;
}
/*
void getLeftSpeed(const std_msgs::Float32& as5048b_left_speed_avg){
    left_speed_avg_val = as5048b_left_speed_avg.data;
    //last_left_speed_time = millis();
}
*/
void getCmd_Vel(const geometry_msgs::Twist& cmd_vel_velocity)
{
  speed_target = cmd_vel_velocity.linear.x;
    prev_cmd_vel_time = millis();
}

//ros::Subscriber <geometry_msgs::Twist> cmd_vel_velocity("/teleop/cmd_vel", getCmd_Vel);
ros::Subscriber <geometry_msgs::Twist> cmd_vel_velocity("/cmd_vel", getCmd_Vel);
//ros::Subscriber <std_msgs::Float32>  as5048b_left_speed_avg("/left_speed_avg", &getLeftSpeed);
std_msgs::UInt16 transmission_PWM_val;  
ros::Publisher transmission_PWM_val_pub("/transmission_PWM", &transmission_PWM_val);

void velocityControl(){
  if ((speed_target >= 0.7501) && (speed_target <= 3.0))            // 4th gear
    {vel_eff_target = speed_settings[6];}
  else if ((speed_target >= 0.5001) && (speed_target <= 0.75))      //3rd gear
    {vel_eff_target = speed_settings[5];}
  else if ((speed_target >= 0.2501) && (speed_target <= 0.5))       //2nd gear
    {vel_eff_target = speed_settings[4];}
  else if ((speed_target >= 0.001) && (speed_target <= 0.25))       //1st gear
    {vel_eff_target = speed_settings[3];}
  else if ((speed_target >= -0.0009) && (speed_target <= 0.0009))   //Neutral
    {vel_eff_target = speed_settings[2];}
  else if ((speed_target <= -0.0009) && (speed_target >= -0.5))     //first_reverse 
    {vel_eff_target = speed_settings[1];}    
  else if ((speed_target <= -0.5001) && (speed_target >= -3.0))   //full_reverse 
    {vel_eff_target = speed_settings[0];}    
  else 
    {
    //Serial.print ("unexpected value: ");
    //Serial.println (speed_target);
  sprintf (buffer, "unexpected cmd_vel speed value:   : %lf", speed_target);
  nh.loginfo(buffer);    
    }

  // write transmission command
  Transmission_Servo.writeMicroseconds(vel_eff_target);
  // update PWM value stored for the transmision servo
  transmission_PWM_val.data = vel_eff_target;   
}

void setup()
{
  nh.initNode();
  delay(1);
  nh.subscribe(cmd_vel_velocity);  // cmd_vel_velocity
//  nh.subscribe(as5048b_left_speed_avg);
  nh.advertise(transmission_PWM_val_pub);
  //Serial.begin(57600);  //115200
  Transmission_Servo.attach(20);  // the SuperServo PWM signal line is on pin 20
  vel_eff_target = vel_neutral;
  Transmission_Servo.writeMicroseconds(vel_eff_target);
  //sprintf (buffer,"speed control - Tractor is connected (last compiled on: %s at: %s ", __DATE__, __TIME__);
  sprintf (buffer,"transmision control is connected (last compiled on: 11/14/21 at: 3:00 pm ");  // this has not been uploaded to the Teensy
  sprintf (buffer, "speed value:   : %lf", vel_eff_target);
  nh.loginfo(buffer); 
}

void loop()
{
    if (millis() - prev_cmd_vel_time >= failsafe_interval) {
        vel_eff_target = vel_neutral;
        //Serial.println(vel_eff_target);
        speed_target = 0;
        Transmission_Servo.writeMicroseconds(vel_eff_target);
        transmission_PWM_val.data = vel_eff_target;   
        }

    if (millis() - prev_time_stamp_velocity >= VelocityInterval) {
      velocityControl();
        prev_time_stamp_velocity = millis();
        }
    if (millis() - prev_time_stamp_info >= infoInterval) {  // provide an informational message to ensure all is OK
        sprintf (buffer, "speed_control - time_delta :%f prev_cmd_vel_time :%lu pwm_value :%d speed_target: %1.2f speed_avg_val: %1.2f ", 
            time_delta, prev_cmd_vel_time, vel_eff_target, speed_target, speed_avg_val);
        nh.loginfo(buffer);         
        prev_time_stamp_info = millis();
        }
    if (millis() - prev_transmission_PWM_time >= transmission_PWM_pub_interval) {
      transmission_PWM_val_pub.publish(&transmission_PWM_val);      // publish PWM value sent to the transmision servo  
      prev_transmission_PWM_time = millis();
      }

    if (millis() - prev_time_stamp_params >= paramsInterval) {  // retrieve paramaters
      nh.getParam("/speed",speed_settings,9);
      full_reverse = speed_settings[0];
      first_reverse = speed_settings[1];
      vel_neutral = speed_settings[2];
      first_forward = speed_settings[3];
      second_forward = speed_settings[4];
      third_forward = speed_settings[5];
      full_forward = speed_settings[6];

      //sprintf (buffer, "speed control - param retrieval - reverse :%1.2f neutral: %1.2f forward: %1.2f fwd nudge: %1.2f rvrs nudge: %1.2f Kp: %1.2f Ki: %1.2f Kd: %1.2f timeout: %1.2f", 
      //    full_reverse, vel_neutral, full_forward, start_forward_nudge, start_reverse_nudge, Kp, Ki, Kd, Ki_timeout);
      sprintf (buffer, "speed control - param retrieval - reverse :%1.2f 1/2 reverse :%1.2f neutral: %1.2f 1st: %1.2f 2nd: %1.2f 3rd: %1.2f 4th: %1.2f", 
          full_reverse, first_reverse, vel_neutral, first_forward, second_forward, third_forward, full_forward);      
      //sprintf (buffer, "speed control - param retrieval - reverse :%d neutral: %d forward: %d fwd nudge: %d rvrs nudge: %d Kp: %d Ki: %d Kd: %d timeout: %d", 
      //    full_reverse, vel_neutral, full_forward, start_forward_nudge, start_reverse_nudge, Kp, Ki, Kd, Ki_timeout);     
      nh.loginfo(buffer);
      prev_time_stamp_params = millis();
    } 
    nh.spinOnce();
    delay(1);
}