/********* Pleasedontcode.com **********

	Pleasedontcode thanks you for automatic code generation! Enjoy your code!

	- Terms and Conditions:
	You have a non-exclusive, revocable, worldwide, royalty-free license
	for personal and commercial use. Attribution is optional; modifications
	are allowed, but you're responsible for code maintenance. We're not
	liable for any loss or damage. For full terms,
	please visit pleasedontcode.com/termsandconditions.

	- Project: test
	- Source Code compiled for: Arduino Nano
	- Source Code created on: 2023-12-03 10:50:30
	- Source Code generated by: Francesco Alessandro

********* Pleasedontcode.com **********/

/****** SYSTEM REQUIREMENTS *****/
/****** SYSTEM REQUIREMENT 1 *****/
	/* read velocity speed and and adjust the servo */
	/* position to maintain the same velocity speed of */
	/* 100km/h. */
/****** END SYSTEM REQUIREMENTS *****/


/********* User code review feedback **********
#### Feedback 1 ####
- you are not using 100km/h target to correct servo position.
#### Feedback 2 ####
- you have to adjust velocity speed read from potentiometer and gi
ve a correction on servo base on target speed of 100km/h.
********* User code review feedback **********/

/****** DEFINITION OF LIBRARIES *****/
#include 

/****** FUNCTION PROTOTYPES *****/
void setup(void);
void loop(void);
void updateInputs(void);
float lookupServoPosition(float velocity);
float map_f(float x, float in_min, float in_max, float out_min, float out_max);
void convertInputsFromRawToPhyData(void);
float lookup_phyData_from_voltage(float voltage, uint8_t segment_points, const float* lookup_table);

/***** DEFINITION OF ANALOG INPUT PINS *****/
const uint8_t pot_Potentiometer_Vout_PIN_A0 = A0;

/***** DEFINITION OF PWM OUTPUT PINS *****/
const uint8_t actuator_Servomotor_PWMSignal_PIN_D3 = 9;

/****** DEFINITION OF ANALOG INPUTS CHARACTERISTIC CURVES *****/
const uint8_t SEGMENT_POINTS_voltage_velocity_PIN_A0 = 5;
const float voltage_velocity_PIN_A0_lookup[2][SEGMENT_POINTS_voltage_velocity_PIN_A0] =
{
  {0.2, 0.8, 2.0, 3.0, 4.0}, //Voltage [V]
  {10.0, 30.0, 60.0, 100.0, 250.0} //velocity [km/h]
};


/***** DEFINITION OF INPUT RAW VARIABLES *****/
/***** used to store raw data *****/
unsigned int pot_Potentiometer_Vout_PIN_A0_rawData = 0; // Analog Input

/***** DEFINITION OF INPUT PHYSICAL VARIABLES *****/
/***** used to store data after characteristic curve transformation *****/
float pot_Potentiometer_Vout_PIN_A0_phyData = 0.0; // velocity [km/h]

/****** DEFINITION OF LIBRARIES CLASS INSTANCES*****/
Servo myservo;

void setup(void)
{
  // put your setup code here, to run once:
  myservo.attach(actuator_Servomotor_PWMSignal_PIN_D3);

  pinMode(pot_Potentiometer_Vout_PIN_A0, INPUT);
}

void loop(void)
{
  // put your main code here, to run repeatedly:
  updateInputs(); // Refresh input data

  convertInputsFromRawToPhyData(); // after that updateInput function is called, so raw data are transformed in physical data in according to characteristic curve

  // Calculate servo position based on velocity
  float targetVelocity = 100.0; // Target velocity speed in km/h
  float targetPosition = lookupServoPosition(targetVelocity);

  // Write the target position to the servo
  myservo.write(targetPosition);

  delay(15);
}

void updateInputs()
{
  pot_Potentiometer_Vout_PIN_A0_rawData = analogRead(pot_Potentiometer_Vout_PIN_A0);
}

/* BLOCK lookupServoPosition */
float lookupServoPosition(float velocity)
{
  // Search table for appropriate value.
  uint8_t index = 0;

  const float *velocityPointer = &voltage_velocity_PIN_A0_lookup[1][0];
  const float *positionPointer = &voltage_velocity_PIN_A0_lookup[0][0];

  // Perform minimum and maximum velocity saturation based on characteristic curve
  velocity = min(velocity, velocityPointer[SEGMENT_POINTS_voltage_velocity_PIN_A0-1]);
  velocity = max(velocity, velocityPointer[0]);

  while( velocityPointer[index] <= velocity && index < SEGMENT_POINTS_voltage_velocity_PIN_A0 )
    index++;

  // If index is zero, velocity value is smaller than our table range
  if( index == 0 )
  {
    return map_f(velocity,
      velocityPointer[0],   // X1
      velocityPointer[1],   // X2
      positionPointer[0],   // Y1
      positionPointer[1]);  // Y2
  }
  // If index is maxed out, velocity value is larger than our range.
  else if( index == SEGMENT_POINTS_voltage_velocity_PIN_A0 )
  {
    return map_f(velocity,
      velocityPointer[SEGMENT_POINTS_voltage_velocity_PIN_A0-2],   // X1
      velocityPointer[SEGMENT_POINTS_voltage_velocity_PIN_A0-1],   // X2
      positionPointer[SEGMENT_POINTS_voltage_velocity_PIN_A0-2],   // Y1
      positionPointer[SEGMENT_POINTS_voltage_velocity_PIN_A0-1]);  // Y2
  }
  // index is between 0 and max, just right
  else
  {
    return map_f(velocity,
      velocityPointer[index-1], // X1
      velocityPointer[index],   // X2
      positionPointer[index-1], // Y1
      positionPointer[index]);  // Y2
  }
}
/* END BLOCK lookupServoPosition */

/* BLOCK map_f */
float map_f(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;
}
/* END BLOCK map_f */

/* BLOCK convertInputsFromRawToPhyData */
void convertInputsFromRawToPhyData()
{
	float voltage = 0.0;

	voltage = pot_Potentiometer_Vout_PIN_A0_rawData * (3.3 / 1023.0);
	pot_Potentiometer_Vout_PIN_A0_phyData = lookup_phyData_from_voltage(voltage, SEGMENT_POINTS_voltage_velocity_PIN_A0, &(voltage_velocity_PIN_A0_lookup[0][0]));

}
/* END BLOCK convertInputsFromRawToPhyData */

float lookup_phyData_from_voltage(float voltage, uint8_t segment_points, const float* lookup_table)
{
  // Implement the lookup function here
  // ...
  return 0.0;
}