Test program for a MAP function aimed at testing code for a steering module that will be used to turn a steering servo from hard left to hard right based on the steering angle provided which will generally be between .82 and -.76 which has to be mapped to a range of -14,000 and 13,500 which are the values from the steering angle sensor.

steer_base_test.ino

/*
Test program for a MAP function aimed at testing code for a steering module that will be used to turn a steering servo from hard left to hard right
based on the steering angle provided which will generally be between .82 and -.76 which has to be mapped to a range of -14,000 and 13,500 which are
the values from the steering angle sensor.

I initially tested  this on an Arduino Nano Every board

   if(angular_vel_z > 0)
    {
        steering_angle_target = angular_vel_z*(steer_left_max - steer_straight) / 1+steer_straight; 
    }
    else if(angular_vel_z < 0)
    {
        steering_angle_target = angular_vel_z*(steer_right_max - steer_straight) / -1+steer_straight; 
    }
    else
    {
        steering_angle_target = steer_straight;   //return steering wheel to middle if there's no command
    }


*/

#include <Arduino.h>

// steering value constants
const int steer_straight = 1;
const int steer_left_max = -14000;
const int steer_right_max = 13500;
const float cmd_vel_full_left = .85;
const float cmd_vel_full_right = -.76;

int steering_target = 0;
//float angular_vel_z;
float angular_vel_z[10] = {.85, .84, .83, .82, 1.0, -.76, -.75, -.74, -1, 0};
int array_size = sizeof(angular_vel_z) / sizeof(float);  // total number of bytes divided by the number of bytes in a float variable
int x = 0;
float steering_angle_target = 0;

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[x] > 0)
    {
        steering_angle_target = angular_vel_z[x]*(steer_left_max - steer_straight) / 1+steer_straight; 
    }
    else if(angular_vel_z[x] < 0)
    {
        steering_angle_target = angular_vel_z[x]*(steer_right_max - steer_straight) / -1+steer_straight; 
    }
    else
    {
        steering_angle_target = steer_straight;   //return steering wheel to middle if there's no command
    }
    Serial.print("x: "); Serial.print(x); Serial.print("   steering_angle_target: "); Serial.println(steering_angle_target);

}

void setup(){
  Serial.begin(9600);
  Serial.print("x: "); Serial.print(x);
  Serial.print("   array_size: "); Serial.print(array_size);
  Serial.print("   max left: "); Serial.print(steer_left_max);
  Serial.print("   max right: "); Serial.println(steer_right_max);
}
void loop(){
  Serial.print("x: "); Serial.print(x); Serial.print("   angular_vel_z: "); Serial.print(angular_vel_z[x]);
  
  if (angular_vel_z[x] < 0)
    {
    if (angular_vel_z[x] < cmd_vel_full_right) {angular_vel_z[x] = cmd_vel_full_right;}
    steering_target = mapfloat(angular_vel_z[x], 0, cmd_vel_full_right, 0, steer_right_max);
    }
    else if (angular_vel_z[x] > 0)
        {
        if (angular_vel_z[x] > cmd_vel_full_left) {angular_vel_z[x] = cmd_vel_full_left;}
        steering_target = mapfloat(angular_vel_z[x], 0, cmd_vel_full_left, 0, steer_left_max);
        }
    else {steering_target = steer_straight;}
   
  Serial.print("   steering_target: "); Serial.println(steering_target);
  steerBase(); 
  delay(3000);
  x = x + 1;
  if (x == array_size){
      Serial.print("x: "); Serial.print(x); Serial.print("   array_size: "); Serial.println(array_size);
      x = 0; 
      delay(30000);
     }
}