// simulate a PWM driven motor controlled by potentiometer
// DaveX 2022-02-21 CC BY-SA
// https://github.com/drf5n/drf5nArduino/tree/main/dc_motor_sim
// Wokwi: https://wokwi.com/projects/323970337959051859
// This simulates a PID+PWM controlled DC motor with the simulated motor
// control read back from the pin 9 OC1A PWM read back from the OCR1A register
//
// input: 0-5V potentiometer on A0 sets RPM 0-1023
// output PWM value to motor
//
// motor simulated by a state space motor in class PmMotor
//
// This sketch does PID control of speed of rotation
// for PID of a PLL on angular position see https://wokwi.com/arduino/projects/324274590349001300
//
// Discussions at:
// https://forum.arduino.cc/t/what-is-the-inverse-of-analogwrite-pin-val/960774/20
// https://forum.arduino.cc/t/why-doesnt-analogwrite-protect-its-writes-to-16bit-registers/961470
// https://forum.arduino.cc/t/motor-rpm-measurement-using-optical-encoder-for-pi-control/960163/13?u=davex
#include <PID_v1.h> // https://playground.arduino.cc/Code/PIDLibrary/
class PmMotor { // Permanent Magnet Motor
public:
// DC PM motor state space model per
// https://ctms.engin.umich.edu/CTMS/index.php?example=MotorSpeed§ion=SystemModeling
// input vars
float V = 5; // voltage (controlled by pwm fraction)
uint8_t pwm_level; // 0-255 pwm controlled voltage
// state vars
float theta = 0; // rotational position (radians)
float theta_dot = 0; // rad/sec
float i_amps = 0 ; // coil current amps
// outputs
float theta_dotdot = 0; // rad/sec/sec
float di_dt;
// extras;
float MaxRPM = 1000;
//constants
float J = 0.001; // inertia kg*m2
float B = 0.0001 ; // fluid friction N*m*s
float L = 0.05; // motor inductance H
float Ke = 0.001; // speed constant V/rad/sec
float Kt = 1; // torque constant N*m/A
float R = 1.0; // motor resistance ohms
float i_full = 0.01; // no load current at full speed voltage
// governing eqns:
// J*theta_dotdot + B * theta_dot = Kt *i
// L * di/dt +R*i = V -Ke*theta_dot
// time variables
unsigned long updateInterval = 50; // ms
unsigned long lastUpdate = 0;
float speed_mrev_sec = 0; //theta_dot
long millirevs = 0; // theta in millirevolutions
float rpm;
void setKeMaxRPMVR(float rpm) {
// set Ke from....
// assume V voltage, for pwm/255 fraction
// assume di/dt =0
// theta_dot_max = rpm/60*2*pi // to rad/sec
// V*255/255 = R*i +Ke*theta_dot
//
Ke = (V - 0.0 * i_full * R) / (rpm / 60 * 2 * PI);
MaxRPM = rpm;
}
void setInertia(float x) {
J = x;
}
void setResistance(float x) {
R = x;
}
void setKtFromIB() {
// From J*theta_dotdot + B * theta_dot = Kt *i
// assume theta_dotdot = 0, full speed, i
const float theta_dot_max = MaxRPM / 60 * 2 * PI ;
float myKt = B * theta_dot_max / i_full;
Serial.print(Kt);
Serial.print("->");
Serial.print(myKt);
Serial.println();
}
void update(uint16_t pwm) {
pwm_level = pwm;
update();
}
void update() {
unsigned long now = millis();
static float i_last = 0;
if (now - lastUpdate >= updateInterval) {
lastUpdate += updateInterval;
i_amps = (V * pwm_level / 255.0 - Ke * theta_dot - L * di_dt) / R;
di_dt = i_amps - i_last;
theta += theta_dot * updateInterval / 1000.0;
theta_dotdot = (Kt * i_amps - B * theta_dot) / J;
theta_dot += theta_dotdot * updateInterval / 1000.0;
}
}
void printConfig(void) { // print the configuration vars
Serial.print("Motor Configuration");
Serial.print(" MaxRPM=");
Serial.print(MaxRPM);
Serial.print(" Ke=");
Serial.print(Ke, 4);
Serial.print("V/rad/sec, Kt=");
Serial.print(Kt, 4);
Serial.print("N*m/A B=");
Serial.print(B, 4);
Serial.print("N*m*s, J=");
Serial.print(J, 4);
Serial.print("kg*m2, L=");
Serial.print(L, 4);
Serial.print("H V=");
Serial.print(V, 4);
Serial.print("V pwm_level=");
Serial.print(pwm_level);
Serial.println('\n');
}
void printState(void) { // Print the state variables
Serial.print(" pwm=");
Serial.print(pwm_level);
Serial.print(" i_amps=");
Serial.print(i_amps);
Serial.print("A theta=");
Serial.print(theta);
Serial.print("rad theta_dot=");
Serial.print(theta_dot);
Serial.print("rad/s theta_dotdot=");
Serial.print(theta_dotdot);
Serial.println("rad/s2");
}
void printStateRPM(void) { //print the state in rev & RPM form
Serial.print(" Motor: pwm=");
Serial.print(pwm_level);
Serial.print(" i=");
Serial.print(i_amps);
Serial.print("A ");
Serial.print(theta / 2 / PI);
Serial.print("rev ");
Serial.print(theta_dot / 2 / PI * 60, 5);
Serial.print("RPM ");
Serial.print(theta_dotdot / 2 / PI * 60, 5);
Serial.print("RPM/s ");
Serial.println();
}
};
PmMotor myMotor;
double myRPM = 0.0;
double mySetpointRPM = 0.0;
double myCV = 0.0;
const byte modePin = 2;
const byte pwmPin = 9;
PID motorPID(&myRPM, &myCV, &mySetpointRPM, 1, 0.5, 0, DIRECT);
void report (void) {
const int interval = 500;
static unsigned long last = - interval;
unsigned long now = millis();
if (now - last >= interval) {
last += interval;
//motor.printState();
Serial.print("PID: SP: ");
Serial.print(mySetpointRPM);
Serial.print("rpm MV: ");
Serial.print(myRPM);
Serial.print("rpm CV: ");
Serial.print(myCV);
Serial.print(" counts PWM9: ");
// Serial.print(OCR1A,DEC);
Serial.print(analogGetPwm9(), DEC);
Serial.println();
myMotor.printStateRPM();
//motor.printConfig();
}
}
void setup() {
// Tweak the inertia and fluid friction up for a slower, easier to control motor
Serial.begin(115200);
myMotor.V = 5; // Volts
myMotor.setResistance(1.2); // Ohms
myMotor.setKeMaxRPMVR(15000); // speed constant V/rad/sec
myMotor.i_full = 0.1; // Amps at no-load full speed
myMotor.B = 0.01e-0; // fluid friction N*m*s // 0.00001e-0
myMotor.setInertia(0.01); // inertia kg*m2 // 0.001
myMotor.setKtFromIB(); // torque constant N*m/A
myMotor.L = 0.05; // motor inductance H
myMotor.printConfig();
myMotor.printState();
pinMode(modePin, INPUT_PULLUP);
pinMode(pwmPin, OUTPUT);
myRPM = myMotor.theta_dot * 60 / 2 / PI;
mySetpointRPM = analogRead(A0); // 0-1024 rpm
//turn the PID on
motorPID.SetSampleTime(10); //us
motorPID.SetMode(AUTOMATIC);
delay(500);
}
int analogGetPwm9() { // PWM 9 on UNO is OCR1A
// Retrieve PWM value from pin per https://forum.arduino.cc/t/what-is-the-inverse-of-analogwrite-pin-val/960774/20
// Uno pin 9 is OC1A per
// Uno https://docs.arduino.cc/hacking/hardware/PinMapping168
// Mega https://docs.arduino.cc/hacking/hardware/PinMapping2560
return ((TCCR1A & bit(COM1A1)) ? OCR1A : digitalRead(pwmPin) * 255);
}
void loop() {
mySetpointRPM = analogRead(A0); // setpoint based on slider
myRPM = myMotor.theta_dot * 60 / 2 / PI; // measure the motor speed
if (digitalRead(modePin) == LOW) {
motorPID.Compute(); // PID to calculate CV from SP and MV
analogWrite(pwmPin, myCV); // push PID control value out PWM pin 9
} else {
analogWrite(pwmPin, mySetpointRPM / 4);
}
// Simulate a motor attached to a PWM pin
myMotor.update(analogGetPwm9());
report();
}
PID / Manual