// ПР5. Управление сервоприводом (ШИМ 50 Гц) и шаговым двигателем (A4988)
// Плата: NUCLEO-C031C6, Wokwi + STM32duino
// Автор: Алексеев Ф.В., ЭФБО-08-23
//
// Сервопривод: TIM3_CH2 на PB5 (D6), f_TIM = 48 МГц
//   PSC = 47  -> f_cnt = 1 МГц (1 тик = 1 мкс)
//   ARR = 19999 -> период 20 мс (50 Гц)
//   CCR2 = длительность импульса в мкс: 544 мкс (0°) … 2400 мкс (180°)
// Шаговый двигатель: драйвер A4988, STEP = D3 (PB3), DIR = D2 (PA10),
//   полношаговый режим, 200 шагов/оборот (1,8°/шаг)

#define SERVO_PIN  D6     // PB5 = TIM3_CH2
#define STEP_PIN   D3
#define DIR_PIN    D2
#define BTN_FWD    D5     // 200 шагов вперёд
#define BTN_REV    D4     // 100 шагов назад
#define POT_ANGLE  A0     // задание угла сервопривода
#define POT_SPEED  A1     // задание скорости шагового двигателя

const uint32_t PSC = 47, ARR = 19999;
const uint16_t PULSE_MIN = 544, PULSE_MAX = 2400;   // мкс
HardwareTimer *servoTim;
uint32_t servoCh;
int servoAngle = -1;

// ---- состояние шагового двигателя ----
long     position = 0;          // текущее положение, шаги
long     stepsLeft = 0;         // осталось сделать шагов
int      dirSign = 1;           // +1 вперёд, -1 назад
uint32_t stepInterval = 2000;   // период шагов, мкс
uint32_t tLastStep = 0;

// Установка угла: пересчёт угла в длительность импульса и запись в CCR2
void servoWrite(int angle) {
  angle = constrain(angle, 0, 180);
  uint32_t us = map(angle, 0, 180, PULSE_MIN, PULSE_MAX);
  servoTim->setCaptureCompare(servoCh, us, TICK_COMPARE_FORMAT);
  servoAngle = angle;
  Serial.print("SERVO angle = "); Serial.print(angle);
  Serial.print(" deg, pulse = "); Serial.print(us);
  Serial.print(" us, CCR2 = "); Serial.println(TIM3->CCR2);
}

// Постановка задания для шагового двигателя
void stepperMove(long steps) {
  dirSign = steps >= 0 ? 1 : -1;
  digitalWrite(DIR_PIN, dirSign > 0 ? HIGH : LOW);   // направление
  stepsLeft = labs(steps);
  Serial.print("STEPPER "); Serial.print(dirSign > 0 ? "FWD " : "REV ");
  Serial.print(stepsLeft); Serial.print(" steps, interval = ");
  Serial.print(stepInterval); Serial.print(" us (");
  uint32_t rpm10 = 3000000UL / stepInterval;          // об/мин x10 = 60e6 / (200*T) * 10
  Serial.print(rpm10 / 10); Serial.print("."); Serial.print(rpm10 % 10); Serial.println(" rpm)");
}

// Неблокирующая генерация импульсов STEP по micros()
void stepperRun() {
  if (stepsLeft == 0) return;
  uint32_t now = micros();
  if (now - tLastStep >= stepInterval) {
    tLastStep = now;
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(3);                 // ширина импульса > 1 мкс (A4988)
    digitalWrite(STEP_PIN, LOW);
    position += dirSign;
    if (--stepsLeft == 0) {
      Serial.print("STEPPER done, position = "); Serial.println(position);
    }
  }
}

// Ожидание с продолжением работы шагового двигателя
void waitRun(uint32_t ms) {
  uint32_t t0 = millis();
  while (millis() - t0 < ms) stepperRun();
}

bool pressed(uint8_t pin, uint8_t i) {
  static bool st[2] = {HIGH, HIGH}, last[2] = {HIGH, HIGH};
  static uint32_t t[2] = {0, 0};
  bool raw = digitalRead(pin);
  if (raw != last[i]) { last[i] = raw; t[i] = millis(); }
  if (millis() - t[i] > 25 && raw != st[i]) { st[i] = raw; return raw == LOW; }
  return false;
}

void setup() {
  Serial.begin(115200);
  analogReadResolution(12);
  pinMode(STEP_PIN, OUTPUT); pinMode(DIR_PIN, OUTPUT);
  pinMode(BTN_FWD, INPUT_PULLUP); pinMode(BTN_REV, INPUT_PULLUP);

  // ШИМ 50 Гц для сервопривода
  PinName pn = digitalPinToPinName(SERVO_PIN);
  TIM_TypeDef *inst = (TIM_TypeDef *)pinmap_peripheral(pn, PinMap_PWM);
  servoCh = STM_PIN_CHANNEL(pinmap_function(pn, PinMap_PWM));
  servoTim = new HardwareTimer(inst);
  servoTim->setMode(servoCh, TIMER_OUTPUT_COMPARE_PWM1, pn);
  servoTim->setPrescaleFactor(PSC + 1);
  servoTim->setOverflow(ARR + 1, TICK_FORMAT);
  Serial.print("Servo PWM: PSC = "); Serial.print(PSC); Serial.print(", ARR = ");
  Serial.print(ARR); Serial.print(", f = ");
  Serial.print(servoTim->getTimerClkFreq() / ((PSC + 1) * (ARR + 1))); Serial.println(" Hz");
  servoWrite(0);
  servoTim->resume();

  // Демонстрация: 0° -> 90° -> 180°, 200 шагов вперёд, 100 назад
  waitRun(1000); servoWrite(90);
  waitRun(1000); servoWrite(180);
  waitRun(1000);
  stepperMove(200);  while (stepsLeft) stepperRun();
  stepperMove(-100); while (stepsLeft) stepperRun();
  Serial.println("Manual mode: A0 - angle, A1 - speed, D5 - FWD 200, D4 - REV 100");
}

void loop() {
  // Угол от потенциометра A0 (с зоной нечувствительности 2°)
  int a = map(analogRead(POT_ANGLE), 0, 4095, 0, 180);
  if (abs(a - servoAngle) >= 2) servoWrite(a);

  // Скорость от потенциометра A1: 500…5000 мкс на шаг (600…60 об/мин)
  stepInterval = map(analogRead(POT_SPEED), 0, 4095, 5000, 500);

  if (pressed(BTN_FWD, 0)) stepperMove(200);
  if (pressed(BTN_REV, 1)) stepperMove(-100);
  stepperRun();
}