Test code for MAP function to be integrated into steering control module

map_function_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.
*/

#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 = .82;
const float cmd_vel_full_right = -.76;

int steering_target;
float sensorValue; 

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 setup(){
  Serial.begin(9600);
}
void loop(){
  sensorValue = .4;  // test values: .82, 1.0, -.76, -1, 0
  Serial.print("sensor value: "); Serial.println(sensorValue);
  if (sensorValue < 0) 
    {
    if (sensorValue < cmd_vel_full_right) {sensorValue = cmd_vel_full_right;}
    steering_target = mapfloat(sensorValue, 0, cmd_vel_full_right, 0, steer_right_max);
    }
    else if (sensorValue > 0) 
        {
        if (sensorValue > cmd_vel_full_left) {sensorValue = cmd_vel_full_left;}
        steering_target = mapfloat(sensorValue, 0, cmd_vel_full_left, 0, steer_left_max); 
        }
    else {
      steering_target = steer_straight;
    }
  Serial.print("steering_target: "); Serial.println(steering_target);      
  delay(1000); 
}