Arduino / Teensy code for ROS serial control of servo

transmission_v1.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:
3/14/20 - add reading parameters - ready to test
3/14/20 - add info statement with last compiled date - ready to test
3/15/20 - change speed error calculation to use the speed average instead of left - ready to test
3/15/20 - add publishing debug statements (e.g. error value) - not sure which ones I need  that I don't already have
3/15/20 - It looks like the PID only has Kp and Ki, no Kd - just using a placeholder at the moment - I would need have to rewrite PID
future - add a right speed reading

*/
#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 prev_transmission_PWM_time = 0;
float prev_time_stamp_info =  0;
float prev_time_stamp_velocity =  0;
float prev_cmd_vel_time = 0;
float time_delta =0;
float 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 full_reverse = 1000;
float vel_neutral = 1290;
float full_forward = 1500;
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 err_value = 0;
float err_value_Kp = 0;
float Kp = 70;
float Ki = 5;
float Kd = 0;
float Ki_timeout = 20;
float err_last = 0;
float error_sum = 0;
int Ki_count = 0;
float speed_settings[9];
//int speed_settings[5];

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;
  //Kp = cmd_vel_velocity.linear.y;  // for tuning only
  //Ki = cmd_vel_velocity.linear.z;  // for tuning only
  //start_forward_nudge = cmd_vel_velocity.angular.x;  // for tuning only
  //start_reverse_nudge = cmd_vel_velocity.angular.y;  // for tuning only
    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(){
  // Slowly increase the servo until the mower starts moving.
  // That will be the start value 
  if (speed_target > .05) {
    // Start with min value for forward
    vel_start = start_forward_nudge; 
  } else if (speed_target < -.05) {
    // Start with min value for reverse
    vel_start = start_reverse_nudge; 
  } else {
    //  Set Transmission to Netural 
    vel_start = 0; 
//    vel_eff_sum = 0;
  }

  speed_avg_val = left_speed_avg_val; 
  // speed_avg_val = (left_speed_avg_val + right_speed_avg_val)/2;   when I implement a right speed monitor
  err_value = speed_target - speed_avg_val;
  err_value_Kp = Kp * err_value;
  err_last = err_last - err_value;
  // err_value_Kd = err_last * Kd;

     
  // Vel I
  
  error_sum = error_sum + err_value;
  vel_effort =  error_sum * Ki;

  vel_eff_target =  vel_effort + vel_start + vel_neutral;
  //vel_eff_target = vel_eff_target + err_value_Kd;
  vel_eff_target = vel_eff_target + err_value_Kp;

  // Add P and D
  //vel_effort = vel_effort + err_value_Kp + err_value_Kd;


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

  // Max motor value range check
  if (vel_eff_target > full_forward){
    vel_eff_target = full_forward;
  } else if ((vel_eff_target < full_forward) && (vel_eff_target > full_reverse)){
    vel_eff_target = abs(vel_eff_target);
  } else if (vel_eff_target < full_reverse){
    vel_eff_target = full_reverse;
  }

  // write transmission command
    if (err_value < -0.01){
      Transmission_Servo.writeMicroseconds(vel_eff_target);
    } else if (err_value > .01){
      Transmission_Servo.writeMicroseconds(vel_eff_target);
    } else {
        vel_eff_target = vel_neutral;
        Transmission_Servo.writeMicroseconds(vel_eff_target);
        }

    // publish PWM value sent to the transmision servo
    transmission_PWM_val.data = vel_eff_target;
    //transmission_PWM_val_pub.publish(&transmission_PWM_val);    

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

}
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: 5/17/20 at: 1:55 pm ");
  nh.loginfo(buffer); 
}

void loop()
{
    if (millis() - prev_cmd_vel_time >= failsafe_interval) {
        vel_eff_target = vel_neutral;
        speed_target = 0;
        Transmission_Servo.writeMicroseconds(vel_eff_target);
        transmission_PWM_val.data = vel_eff_target;
        //transmission_PWM_val_pub.publish(&transmission_PWM_val);      // publish PWM value sent to the transmision servo     
        }

    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 :%f pwm_value :%d speed_target: %1.2f speed_avg_val: %1.2f Kp: %1.2f  Ki: %1.2f  start_forward_nudge %1.2f ", 
            time_delta, prev_cmd_vel_time, vel_eff_target, speed_target, speed_avg_val, Kp, Ki, start_forward_nudge);
        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("/steering", steer_settings,5);
      nh.getParam("/speed",speed_settings,9);
      //nh.getParam("/speed", speed_settings, 5);
      full_reverse = speed_settings[0];
      vel_neutral = speed_settings[1];
      full_forward = speed_settings[2];
      start_forward_nudge = speed_settings[3];
      start_reverse_nudge = speed_settings[4];
      Kp = speed_settings[5];
      Ki = speed_settings[6];
      Kd = speed_settings[7];
      Ki_timeout = speed_settings[8];
      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 :%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);
}