drake/dmx-motor-controller
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

new version with PID and calc velocity

Browse Source
This commit is contained in:
Drake Marino 2024-10-18 20:32:35 -05:00
parent 3686061e3a
commit 547c2f3569
1 changed files with 183 additions and 198 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

379
src/main.cpp
View File

@ -1,5 +1,5 @@
#include <Arduino.h> #include <Arduino.h>
//#include <DMXSerial.h> #include <DMXSerial.h>
#include <PID_v1.h> #include <PID_v1.h>
#include <PWM.h> #include <PWM.h>
@ -17,34 +17,46 @@
#define LLED_PIN 5 #define LLED_PIN 5
#define RLED_PIN 6 #define RLED_PIN 6
#define ENCODER_PPR 6000 // pulses per revolution #define ENCODER_PPR 600 // pulses per revolution
#define ENCODER_RATIO 1.0f // number of times encoder spins per revolution of wall #define ENCODER_RATIO 2.0f // number of times encoder spins per revolution of wall
#define MOTOR_RATIO 5.0f // number of time motor spins per revolution of wall #define MOTOR_RATIO 10.0f // number of time motor spins per revolution of wall
#define MOTOR_MAX_RPM 330.0f // p/n MY1016Z3 rated 300 rpm @ 24VDC; enter measured value #define MOTOR_MAX_RPM 330.0f // p/n MY1016Z3 rated 300 rpm @ 24VDC; enter measured value
#define MOTOR_MIN_PWM 30 // enter value required for motor to start turning #define MOTOR_MIN_PWM 20 // enter value required for motor to start turning
#define MAX_MOTOR_ACCELERATION (MOTOR_MAX_RPM / 5.0f) // (RPM/s): 0 to max in 0.5s #define MAX_MOTOR_ACCELERATION (MOTOR_MAX_RPM / 5.0f) // (RPM/s): 0 to max in 0.5s
#define MAX_WALL_ACCELERATION MAX_MOTOR_ACCELERATION / MOTOR_RATIO #define MAX_WALL_ACCELERATION MAX_MOTOR_ACCELERATION / MOTOR_RATIO
#define WALL_POSITION_TOLERANCE 0.00056 // acceptable error in wall position (2 degrees = 0.0056 rev) #define SPEEDRAMP_DISTANCE 0.3f // range target velocity should be proportional to error (10 degrees = 0.0278 rev)
#define WALL_POSITION_TOLERANCE 0.0056f // acceptable error in wall position (2 degrees = 0.0056 rev)
#define SAMPLE_RATE 100.0f // min time (ms) between velocity calculations #define SAMPLE_RATE 10000 // min time (us) between velocity calculations
#define LOOP_RATE 100.0f // min time (ms) between control adjustments. ** Must be >= SAMPLE_RATE ** #define LOOP_RATE 10 // min time (ms) between control adjustments. ** Must be >= SAMPLE_RATE (converted to us) **
const int DMX_CHANNEL = 1; #define DMX_CHANNEL 500 // DMX Channel to read
long encoderPosition = 0; // raw count from encoder // wall state variables:
double maxWallVelocity = MOTOR_MAX_RPM / MOTOR_RATIO; long encoderPosition = 0; // raw count from encoder
double currentPosition = 0; // wall (revolutions) // double maxWallVelocity = MOTOR_MAX_RPM / MOTOR_RATIO;
double prevPosition = 0; // wall (revolutions) double currentPosition = 0; // wall (revolutions)
double targetPosition = 0; // wall (revolutions) double prevPosition = 0; // wall (revolutions)
double currentVelocity = 0; // wall velocity (rpm) double targetPosition = 0; // wall (revolutions)
double targetMotorVelocity = 0; // motor velocity (rpm) double currentVelocity = 0; // wall velocity (rpm)
unsigned long prevSample = 0; // time tracker for sensor sampling rate // motor control variables:
unsigned long prevControl = 0; // time tracker for control loop double currentMotorVelocity = 0; // motor velocity (rpm)
double remainingDistance = 0; // wall distance from current to target (revolutions) double targetMotorVelocity = 0; // motor velocity (rpm)
double remainingTime = 0; // estimate of time to get from current position to target (s)
double stopTime = 0; // estimate of time to decelerate to zero unsigned long prevSample = 0; // time tracker for sensor sampling rate (us)
int motorPWM = 0; // motor PWM level to achieve target motor velocity (0-255) unsigned long prevControl = 0; // time tracker for control loop (ms)
double remainingDistance = 0; // wall distance from current to target (revolutions)
double remainingTime = 0; // estimate of time to get from current position to target (s)
double stopTime = 0; // estimate of time to decelerate to zero
double motorPWM = 0; // motor PWM command level (-255/+255)
double commandVelocity = 0; // PID commanded motor velocity (RPM)
//debug variables:
// int prevPWM = 0; // motor command from previous loop
// int newPWM = 0; // updated motor command
// int dummyTarget = 0;
// long lastToggle = 0;
// Serial Input processor: // Serial Input processor:
const byte numChars = 32; const byte numChars = 32;
@ -55,17 +67,20 @@ boolean newData = false;
void updateEncoder(); // react to encoder pulse and update encoder count void updateEncoder(); // react to encoder pulse and update encoder count
double encoderToPosition(long); // convert encoder count to wall position double encoderToPosition(long); // convert encoder count to wall position
int Velocity_to_PWM(double); // function to convert motor velocity to PWM drive signal int Velocity_to_PWM(double); // function to convert motor velocity to PWM drive signal
void recvWithEndMarker(); // read Serial input // bool recvWithEndMarker(); // read Serial input
void showNewData(); // act on Serial input // float readNewTarget(); // convert Serial input to new target position
int sign(float); // get sign of a value: (returns +1 or -1)
PID pid = PID(&currentPosition, &targetMotorVelocity, &targetPosition, 600, 0.25, 5.0, DIRECT); //PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, ControllerDirection)
PID pid = PID(&currentMotorVelocity, &commandVelocity, &targetMotorVelocity, 255.0f/330.0f, 0.0, 0.0, DIRECT);
// PID SetSampleTime((int) LOOP_RATE);
void setup() { void setup() {
Serial.begin(115200); // Serial.begin(115200);
// Configure DMX shield (MAX485 chipset) // Configure DMX shield (MAX485 chipset)
// DMXSerial.init(DMXReceiver); DMXSerial.init(DMXReceiver);
// DMXSerial.write(DMX_CHANNEL, 0); DMXSerial.write(DMX_CHANNEL, 0);
// Initialize GPIO pins: // Initialize GPIO pins:
pinMode(RPWM_PIN, OUTPUT); pinMode(RPWM_PIN, OUTPUT);
@ -83,171 +98,140 @@ void setup() {
// Start the Motor Driver - Set Frequency, start both PWM low, Enable both sides: // Start the Motor Driver - Set Frequency, start both PWM low, Enable both sides:
InitTimersSafe(); InitTimersSafe();
SetPinFrequencySafe(9, 10000); SetPinFrequencySafe(9, 10000);
SetPinFrequencySafe(10, 10000);
digitalWrite(LPWM_PIN, LOW); digitalWrite(LPWM_PIN, LOW);
digitalWrite(RPWM_PIN, LOW); digitalWrite(RPWM_PIN, LOW);
digitalWrite(L_EN_PIN, HIGH); digitalWrite(L_EN_PIN, HIGH);
digitalWrite(R_EN_PIN, HIGH); digitalWrite(R_EN_PIN, HIGH);
pid.SetSampleTime(10); pid.SetSampleTime(LOOP_RATE);
pid.SetMode(AUTOMATIC); pid.SetMode(AUTOMATIC);
pid.SetOutputLimits(-330, 330); pid.SetOutputLimits(-330, 330);
// initialize loop timers // initialize loop timers
prevSample = millis(); prevSample = millis();
prevControl = millis(); prevControl = millis();
// lastToggle = millis() - 2000;
} }
void loop() { void loop() {
if (millis() < prevControl) { // if (millis() - lastToggle > 7000) {
// millis timer rolled over, so reset sample and control timers: // if (targetPosition == 0) {
prevSample = millis(); // targetPosition = 0.5;
prevControl = millis(); // } else {
currentPosition = encoderToPosition(encoderPosition); // targetPosition = 0;
} // }
// lastToggle = millis();
// }
if (millis() - prevSample >= SAMPLE_RATE) { if (DMXSerial.noDataSince() < 5000) {
// this happens faster than control loop so that TBD smoothing function can be added digitalWrite(R_EN_PIN, HIGH);
prevPosition = currentPosition; digitalWrite(L_EN_PIN, HIGH);
currentPosition = encoderToPosition(encoderPosition); // get wall position (revolutions) from encoder reading
currentVelocity = 60000.0f * (currentPosition - prevPosition) / (float)(millis() - prevSample); // wall speed (rpm)
prevSample = millis();
}
if (millis() - prevControl >= LOOP_RATE) { if (micros() < prevSample) {
// targetMotorVelocity = currentVelocity * MOTOR_RATIO; // begin with assumption we are going the right speed // micros timer rolled over, so reset sample timer:
double remainingDistance = targetPosition - currentPosition; // Serial.println("micros() roll-over");
// double remainingTime; // projected running time assuming velocity doesn't change prevSample = micros();
currentPosition = encoderToPosition(encoderPosition);
} else if (micros() - prevSample >= SAMPLE_RATE) {
// this can happen faster than control loop so that (TBD) smoothing function can be added
prevPosition = currentPosition;
double dt = (float (micros() - prevSample)) / (1.0e6f); // time since last sample (s)
currentPosition = encoderToPosition(encoderPosition); // get wall position (rev) from encoder reading
currentVelocity = 60.0f * (currentPosition - prevPosition) / dt; // wall speed (rpm)
currentMotorVelocity = currentVelocity * MOTOR_RATIO; // motor speed (rpm), assuming no slipping
remainingDistance = targetPosition - currentPosition; // position error (rev)
if (currentVelocity == 0) {
remainingTime = 1e6;
} else {
remainingTime = remainingDistance/currentVelocity;
}
// if (currentVelocity == 0) { if (abs(remainingDistance) < WALL_POSITION_TOLERANCE) {
// remainingTime = 1e6; // we are close enough, stop moving:
// } else { targetMotorVelocity = 0.0f;
// remainingTime = remainingDistance / currentVelocity; remainingTime = 0.0f;
// } } else {
// if (abs(remainingDistance) < WALL_POSITION_TOLERANCE) { targetMotorVelocity = sign(remainingDistance) * min(abs(remainingDistance) / SPEEDRAMP_DISTANCE * MOTOR_MAX_RPM, MOTOR_MAX_RPM);
// // we are close enough, stop moving: }
// if (abs(targetMotorVelocity) < MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f) {
// // we aren't going very fast so just command zero:
// targetMotorVelocity = 0;
// } else {
// // correct by max allowable rate:
// targetMotorVelocity =- (abs(targetMotorVelocity) / targetMotorVelocity) * MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f;
// }
// } else if (remainingTime < 0) {
// // moving in the wrong direction, so course correct:
// if (remainingDistance > 0) {
// // need to go CCW, so target velocity is more positive than current velocity
// // but if we are close enough, don't over accelerate; just calculate target speed
// targetMotorVelocity = min(targetMotorVelocity + MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f, (0.5 * remainingDistance / ((float)LOOP_RATE/1000.0f)) * MOTOR_RATIO);
// } else {
// // need to go CW, so target velocity is more negative than current velocity
// targetMotorVelocity = max(targetMotorVelocity - MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f, (0.5 * remainingDistance / ((float)LOOP_RATE/1000.0f)) * MOTOR_RATIO);
// }
// } else {
// // moving in the correct direction, so check if we should start slowing down:
// stopTime = abs(currentVelocity) / MAX_WALL_ACCELERATION;
// if (stopTime > (remainingTime - 2*(float)LOOP_RATE/1000.0f)) {
// // we need to be slowing down within the next two control loops, so start now:
// if (remainingDistance > 0) {
// // going CCW, so target should adjust in CW direction
// targetMotorVelocity = targetMotorVelocity - MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f;
// if (targetMotorVelocity < 0) {
// targetMotorVelocity = 0;
// }
// } else {
// // going CW, so target should adjust in CCW direction
// targetMotorVelocity = targetMotorVelocity + MAX_MOTOR_ACCELERATION * (float)LOOP_RATE/1000.0f;
// if (targetMotorVelocity > 0) {
// targetMotorVelocity = 0;
// }
// }
// } else {
// // we still have a ways to go, so we can accelerate:
// if (remainingDistance > 0) {
// // going CCW, so target should adjust more CCW until MAX
// targetMotorVelocity = min(MOTOR_MAX_RPM, targetMotorVelocity + MAX_MOTOR_ACCELERATION * LOOP_RATE/1000.0f);
// } else {
// // going CW, so target should adjust more CW until -MAX
// targetMotorVelocity = max(-MOTOR_MAX_RPM, targetMotorVelocity - MAX_MOTOR_ACCELERATION * LOOP_RATE/1000.0f);
// }
// }
// }
// prevPWM = motorPWM;
prevSample = micros();
// int motorPWM = Velocity_to_PWM(targetMotorVelocity); /*
// targetMotorVelocity += min((remainingDistance * 0.5) - targetMotorVelocity, MAX_MOTOR_ACCELERATION / LOOP_RATE / 1000.0f); Serial.print("sample updated:");
Serial.print(" pos: ");Serial.print(currentPosition);
// if (abs(remainingDistance) > WALL_POSITION_TOLERANCE) { Serial.print(" vel: ");Serial.print(currentVelocity);
// if (((remainingDistance / 0.16 * 330) - targetMotorVelocity) > 0) { Serial.print(" error: ");Serial.print(remainingDistance);
// targetMotorVelocity = currentVelocity + min((remainingDistance / 0.16 * 330) - currentVelocity, MAX_MOTOR_ACCELERATION * (LOOP_RATE / 1000.0f)); Serial.println();
// } else { */
// targetMotorVelocity = currentVelocity + max((remainingDistance / 0.16 * 330) - currentVelocity, - MAX_MOTOR_ACCELERATION * (LOOP_RATE / 1000.0f));
// }
// if (targetMotorVelocity > MOTOR_MAX_RPM) {
// targetMotorVelocity = MOTOR_MAX_RPM;
// }
// if (targetMotorVelocity < - MOTOR_MAX_RPM) {
// targetMotorVelocity = - MOTOR_MAX_RPM;
// }
// } else if (currentVelocity < 1){
// targetMotorVelocity = 0;
// }
pid.Compute();
int motorPWM = Velocity_to_PWM(targetMotorVelocity);
if (targetMotorVelocity == 0) {
digitalWrite(RPWM_PIN, LOW);
digitalWrite(RLED_PIN, LOW);
digitalWrite(LPWM_PIN, LOW);
digitalWrite(LLED_PIN, LOW);
} else if (targetMotorVelocity > 0) {
digitalWrite(RPWM_PIN, LOW);
digitalWrite(RLED_PIN, LOW);
pwmWrite(LPWM_PIN, motorPWM);
pwmWrite(LLED_PIN, motorPWM);
} else {
digitalWrite(LPWM_PIN, LOW);
digitalWrite(LLED_PIN, LOW);
pwmWrite(RPWM_PIN, motorPWM);
pwmWrite(RLED_PIN, motorPWM);
} }
prevControl = millis(); if (pid.Compute()) {
// newPWM = motorPWM;
Serial.print(">ePos: "); motorPWM = Velocity_to_PWM(commandVelocity);
Serial.print(encoderPosition);
Serial.print(", wPos: ");
Serial.print(currentPosition);
Serial.print(", wVel: ");
Serial.print(currentVelocity);
Serial.print(", rDist: ");
Serial.print(remainingDistance);
Serial.print(", rTime: ");
Serial.print(remainingTime);
Serial.print(", sTime:");
Serial.print(stopTime);
Serial.print(", tVel: ");
Serial.print(targetMotorVelocity);
Serial.print(", mPWM: ");
Serial.println(motorPWM);
// update motor controller command:
if (abs(motorPWM) < MOTOR_MIN_PWM) {
// Serial.println("PWM < MIN");
digitalWrite(RPWM_PIN, LOW);
digitalWrite(RLED_PIN, LOW);
digitalWrite(LPWM_PIN, LOW);
digitalWrite(LLED_PIN, LOW);
} else if (motorPWM > 0) {
// Serial.println("PWM > 0");
digitalWrite(RPWM_PIN, LOW);
digitalWrite(RLED_PIN, LOW);
pwmWrite(LPWM_PIN, motorPWM);
pwmWrite(LLED_PIN, motorPWM);
} else {
// Serial.println("Else");
digitalWrite(LPWM_PIN, LOW);
digitalWrite(LLED_PIN, LOW);
pwmWrite(RPWM_PIN, -motorPWM);
pwmWrite(RLED_PIN, -motorPWM);
}
// generate some debug output:
// Serial.print(">time: ");
// Serial.print(millis());
// Serial.print(", ePos: ");
// Serial.print(encoderPosition);
// Serial.print(", wPos: ");
// Serial.print(currentPosition);
// Serial.print(", tPos: ");
// Serial.print(targetPosition);
// Serial.print(", wVel: ");
// Serial.print(currentVelocity);
// Serial.print(", rDist: ");
// Serial.print(remainingDistance);
// //Serial.print(", rTime: ");
// //Serial.print(remainingTime);
// Serial.print(", sTime:");
// Serial.print(stopTime);
// Serial.print(", tVel: ");
// Serial.print(targetMotorVelocity);
// Serial.print(", mPWM: ");
// Serial.println(motorPWM);
}
// make sure DMX is still alive:
} else {
digitalWrite(R_EN_PIN, LOW);
digitalWrite(L_EN_PIN, LOW);
} }
recvWithEndMarker(); // react to Serial input (new target position):
showNewData(); // if (recvWithEndMarker()) {targetPosition = readNewTarget();}
// delay(5);
// make sure DMX is still alive:
// // } else {
// // digitalWrite(R_EN_PIN, 0);
// // digitalWrite(L_EN_PIN, 0);
// // }
} }
// return +1 if positive or -1 if negative
int sign(float val) {
// return +1 if positive or -1 if negative
if (val >= 0) {return 1;} else {return -1;}
}
// got an interrupt on ENCODER_A, so update encoder position
void updateEncoder() { void updateEncoder() {
if (digitalRead(ENCODER_B) == LOW) { if (digitalRead(ENCODER_B) == LOW) {
encoderPosition--; // CCW encoderPosition--; // CCW
@ -260,40 +244,41 @@ double encoderToPosition(long encoderPosition) {
return (encoderPosition / (float)ENCODER_PPR) / (float)ENCODER_RATIO; return (encoderPosition / (float)ENCODER_PPR) / (float)ENCODER_RATIO;
} }
int Velocity_to_PWM(double motorVelocity) { int Velocity_to_PWM(double commandVelocity) {
if (abs(motorVelocity) > 0) { if (abs(commandVelocity) > 1) {
return min(round(MOTOR_MIN_PWM + ((255 - MOTOR_MIN_PWM) / (float)MOTOR_MAX_RPM) * abs(motorVelocity)), 255); return min(round(MOTOR_MIN_PWM + ((255 - MOTOR_MIN_PWM) / (float)MOTOR_MAX_RPM) * abs(commandVelocity)), 255) * sign(commandVelocity);
} }
return 0; return 0;
} }
void recvWithEndMarker() { // bool recvWithEndMarker() {
static byte ndx = 0; // static byte ndx = 0;
char endMarker = '\n'; // char endMarker = '\n';
char rc; // char rc;
while (Serial.available() > 0 && newData == false) { // while (Serial.available() > 0 && newData == false) {
rc = Serial.read(); // rc = Serial.read();
if (rc != endMarker) { // if (rc != endMarker) {
receivedChars[ndx] = rc; // receivedChars[ndx] = rc;
ndx++; // ndx++;
if (ndx >= numChars) { // if (ndx >= numChars) {
ndx = numChars - 1; // ndx = numChars - 1;
} // }
} // }
else { // else {
receivedChars[ndx] = '\0'; // terminate the string // receivedChars[ndx] = '\0'; // terminate the string
ndx = 0; // ndx = 0;
newData = true; // newData = true;
} // }
} // }
} // return newData;
// }
void showNewData() { // float readNewTarget() {
if (newData == true) { // Serial.print("New Target Position: ");
Serial.print("This just in ... "); // Serial.println(receivedChars);
Serial.println(receivedChars); // delay(500);
newData = false; // newData = false;
} // return atof(receivedChars);
} // }