/*
    1. 所需硬件：
      - Arduino Uno开发板 *1
      - 面包板 *1
      - 杜邦线 *若干
      - 测速传感器模块 *1
      - 电位器 *1
      - L298n电机驱动 *1
      - 电机 *1

  作者：CT-LAB
*/


/*--------------------------------------------------------------------------1--------------------------------------------------------------------------*/
/*--------------------------------------------------------------------本程序所引用库--------------------------------------------------------------------*/
// 无


/*--------------------------------------------------------------------------2--------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------声明定义-----------------------------------------------------------------------*/
/*****************************< 常量声明 >*****************************/
#define WHEEL_DIAMETER 0.45  // 车轮直径
#define TIMES_PER_SECOND 20  // 串口每秒输出20次

/*****************************< 引脚定义 >*****************************/
const int Potentiometer_Pin = A2;  // 电位器模块信号脚
const int Velocity_Pin = 3;        // 设置车速传感器模块引脚
const int Motor_A = 2;             // 电机引脚1
const int Motor_B = 4;             // 电机引脚2
const int Motor_EN = 5;            // 注意EN需要连接PWM引脚进行调速


/*--------------------------------------------------------------------------3--------------------------------------------------------------------------*/
/*-------------------------------------------------------------------setup()程序初始化------------------------------------------------------------------*/

void setup() {
  /******************************< 启动串口通讯 >******************************/
  Serial.begin(9600);  // 初始化串口通信，并设置波特率为9600

  /******************************< 设置引脚模式 >******************************/
  pinMode(Velocity_Pin, INPUT);  // 设置速度传感器引脚为输入模式
  pinMode(Motor_A, OUTPUT);      // 设置电机控制引脚为输出模式
  pinMode(Motor_B, OUTPUT);      // 设置电机控制引脚为输出模式
  pinMode(Motor_EN, OUTPUT);     // 设置电机PWM引脚为输出模式
}


/*--------------------------------------------------------------------------4--------------------------------------------------------------------------*/
/*-------------------------------------------------------------------loop()基础循环体-------------------------------------------------------------------*/
void loop() {
  float Demand = read_Potentiometer(Potentiometer_Pin);
  float Speed = calculate_Speed(Velocity_Pin, WHEEL_DIAMETER);
  run_Motor(Demand);
  print_Formatted_Output(Speed, Demand, TIMES_PER_SECOND);
}




/*--------------------------------------------------------------------------5--------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------函数定义-----------------------------------------------------------------------*/

float read_Potentiometer(int potPin) {
  float analogValue = analogRead(potPin);                 // 读取电位器的模拟值（0-1023）
  float mappedValue = map(analogValue, 0, 1023, 0, 100);  // 将0-1023范围的值映射到0-100
  return mappedValue;
}

float calculate_Speed(int sensorPin, float wheelDiameter) {
  unsigned long duration = pulseIn(sensorPin, HIGH);  // 读取脉冲长度，单位为微秒
  // 如果 duration 为 0，说明没有检测到脉冲，返回速度 0
  if (duration == 0) {
    return 0.0;
  }

  float speedMetersPerSecond = (PI * wheelDiameter) / (duration / 1000000.0);  // 计算速度 m/s
  float speedKmh = speedMetersPerSecond * 3.6;                                 // 转换为 km/h

  return speedKmh;
}
void run_Motor(int Speed) {
  // 设置电机的旋转
  digitalWrite(Motor_A, HIGH);
  digitalWrite(Motor_B, LOW);

  // 设置电机的速度
  Speed = map(Speed, 0, 100, 0, 255);  // 将0-1023范围的值映射到0-255
  analogWrite(Motor_EN, abs(Speed));  // 控制电机速度
}
void print_Formatted_Output(float speed, float potentiometerValue, int timesPerSecond) {

  static unsigned long previousMillis = 0;           // 静态变量用于存储上一次打印的时间，只在第一次调用时初始化
  long interval = 1000 / timesPerSecond;             // 计算时间间隔（单位：毫秒）
  unsigned long currentMillis = millis();            // 获取当前时间
  if (currentMillis - previousMillis >= interval) {  // 检查是否已经到达设定的时间间隔
    previousMillis = currentMillis;                  // 更新上一次打印的时间
    // 使用串口打印格式化输出，保留速度两位小数，百分比两位小数
    Serial.print("Speed = ");
    Serial.print(speed, 2);  // 保留两位小数
    Serial.print(" km/h \t demand=");
    Serial.print(potentiometerValue, 2);  // 保留两位小数
    Serial.println("%");
  }
}