// speedSpeedometerStepperServo.pde
// -*- mode: C++ -*-
//
// Use a Potentiometer to control the speed of several motors:
// a stepper motor with drivers with speed proportional to pot
// a stepper motor with drivers with position proportional to pot
// a servo motor porportional to pot
// a led/DC motor proportional to pot
// a led/DC motor proportional and opposite to pot
// leds/reversibleDC motor proportional to bidirectioal pot 
//

// Sim: https://wokwi.com/projects/330649945363186260
// Discussion: https://discord.com/channels/787627282663211009/787630013658824707/957769150556667954
//         and https://github.com/wokwi/wokwi-features/issues/191
// Docs:  https://docs.wokwi.com/parts/wokwi-stepper-motor
//   
// See also: https://wokwi.com/projects/327381547863769683 W/o drivers
//           https://wokwi.com/projects/327613078439985746 MultiStepper
// 

#include <AccelStepper.h> // http://www.airspayce.com/mikem/arduino/AccelStepper/
#include <Servo.h>  // https://www.arduino.cc/reference/en/libraries/servo/
                    // Note that servo disables pwm on 9&10

//Connections
const byte dcPwmPin = 3;  // motor driver PWM
const byte dcDirPin = 4;  // motor driver direction
const byte ledUpPin = 5;  // motor driver PWM 0-100% mapping 
const byte ledDownPin = 6; // motor driver 100%-0% mapping
const byte dirPinSpeedometer = 7; // stepper as speed-indicating pot
const byte stepPinSpeedometer = 8; //
const byte servoPin = 9;
const byte dirPin = 11;  // varaible speed stepper motor
const byte stepPin = 12;
const byte potPin = A0;  // Potentiometer input

//const bool useDeadband = true;
const int deadband = 200;
const int step_per_rev = 200;

AccelStepper speedometer(AccelStepper::DRIVER, stepPinSpeedometer, dirPinSpeedometer);
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);
Servo myservo;  // create servo object to control a servo

int potState = 510;
float motorSpeed = 0;
float motorRPM = 0;
float maxRPM = 50;

void setup() {
  pinMode(dcPwmPin, OUTPUT);
  pinMode(dcDirPin, OUTPUT);
  pinMode(ledUpPin, OUTPUT);
  pinMode(ledDownPin, OUTPUT);
  Serial.begin(115200);

  stepper.setMaxSpeed(2000);
  stepper.setSpeed(0);
  speedometer.setMaxSpeed(2000);
  speedometer.setAcceleration(20000);
  myservo.attach(servoPin);
}

void loop() {
  int pot_val = analogRead(A0);
  if (pot_val != potState ) {
    potState = pot_val;
    analogWrite(ledUpPin,potState/4);
    analogWrite(ledDownPin, (int)(255 -potState/4));
    myservo.write(potState * 180UL / 1023);
    if (deadband) {
      if (potState < (1023-deadband)/2) { 
        digitalWrite(dcDirPin,LOW);
        motorRPM = (potState - (1023L-deadband)/2) * maxRPM / ((1023-deadband)/2);
      } else if (potState > (1023+deadband)/2) {
        digitalWrite(dcDirPin,HIGH);
        motorRPM = (potState - (1023L+deadband)/2) * maxRPM / ((1023-deadband)/2);
      } else motorRPM = 0;
    } else {
      motorRPM = map(potState, 0, 1023, -maxRPM, maxRPM); // no deadband
    }

    motorSpeed = motorRPM * step_per_rev / 60;
    speedometer.moveTo(motorRPM);
    stepper.setSpeed(motorSpeed);

    analogWrite(dcPwmPin, floor(abs(potState-511.5)/2) );
    Serial.print(potState);
    Serial.print("->");
    Serial.print(motorRPM);
    Serial.print(" ");
  }

  stepper.runSpeed();
  speedometer.run();
}