From 547c2f3569f5495bc218fc49fcf5972278f3f8b5 Mon Sep 17 00:00:00 2001 From: drake Date: Fri, 18 Oct 2024 20:32:35 -0500 Subject: [PATCH] new version with PID and calc velocity --- src/main.cpp | 381 +++++++++++++++++++++++++-------------------------- 1 file changed, 183 insertions(+), 198 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index f9aeccb..637cbb5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,5 +1,5 @@ #include -//#include +#include #include #include @@ -17,34 +17,46 @@ #define LLED_PIN 5 #define RLED_PIN 6 -#define ENCODER_PPR 6000 // pulses per revolution -#define ENCODER_RATIO 1.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_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 ENCODER_PPR 600 // pulses per revolution +#define ENCODER_RATIO 2.0f // number of times encoder 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_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_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 LOOP_RATE 100.0f // min time (ms) between control adjustments. ** Must be >= SAMPLE_RATE ** +#define SAMPLE_RATE 10000 // min time (us) between velocity calculations +#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 -double maxWallVelocity = MOTOR_MAX_RPM / MOTOR_RATIO; -double currentPosition = 0; // wall (revolutions) -double prevPosition = 0; // wall (revolutions) -double targetPosition = 0; // wall (revolutions) -double currentVelocity = 0; // wall velocity (rpm) -double targetMotorVelocity = 0; // motor velocity (rpm) +// wall state variables: +long encoderPosition = 0; // raw count from encoder +// double maxWallVelocity = MOTOR_MAX_RPM / MOTOR_RATIO; +double currentPosition = 0; // wall (revolutions) +double prevPosition = 0; // wall (revolutions) +double targetPosition = 0; // wall (revolutions) +double currentVelocity = 0; // wall velocity (rpm) -unsigned long prevSample = 0; // time tracker for sensor sampling rate -unsigned long prevControl = 0; // time tracker for control loop -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 -int motorPWM = 0; // motor PWM level to achieve target motor velocity (0-255) +// motor control variables: +double currentMotorVelocity = 0; // motor velocity (rpm) +double targetMotorVelocity = 0; // motor velocity (rpm) + +unsigned long prevSample = 0; // time tracker for sensor sampling rate (us) +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: const byte numChars = 32; @@ -55,17 +67,20 @@ boolean newData = false; void updateEncoder(); // react to encoder pulse and update encoder count double encoderToPosition(long); // convert encoder count to wall position int Velocity_to_PWM(double); // function to convert motor velocity to PWM drive signal -void recvWithEndMarker(); // read Serial input -void showNewData(); // act on Serial input +// bool recvWithEndMarker(); // read 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(¤tPosition, &targetMotorVelocity, &targetPosition, 600, 0.25, 5.0, DIRECT); +//PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, ControllerDirection) +PID pid = PID(¤tMotorVelocity, &commandVelocity, &targetMotorVelocity, 255.0f/330.0f, 0.0, 0.0, DIRECT); +// PID SetSampleTime((int) LOOP_RATE); void setup() { - Serial.begin(115200); + // Serial.begin(115200); // Configure DMX shield (MAX485 chipset) - // DMXSerial.init(DMXReceiver); - // DMXSerial.write(DMX_CHANNEL, 0); + DMXSerial.init(DMXReceiver); + DMXSerial.write(DMX_CHANNEL, 0); // Initialize GPIO pins: pinMode(RPWM_PIN, OUTPUT); @@ -83,171 +98,140 @@ void setup() { // Start the Motor Driver - Set Frequency, start both PWM low, Enable both sides: InitTimersSafe(); SetPinFrequencySafe(9, 10000); + SetPinFrequencySafe(10, 10000); digitalWrite(LPWM_PIN, LOW); digitalWrite(RPWM_PIN, LOW); digitalWrite(L_EN_PIN, HIGH); digitalWrite(R_EN_PIN, HIGH); - pid.SetSampleTime(10); + pid.SetSampleTime(LOOP_RATE); pid.SetMode(AUTOMATIC); pid.SetOutputLimits(-330, 330); // initialize loop timers prevSample = millis(); prevControl = millis(); + // lastToggle = millis() - 2000; } void loop() { - if (millis() < prevControl) { - // millis timer rolled over, so reset sample and control timers: - prevSample = millis(); - prevControl = millis(); - currentPosition = encoderToPosition(encoderPosition); - } + // if (millis() - lastToggle > 7000) { + // if (targetPosition == 0) { + // targetPosition = 0.5; + // } else { + // targetPosition = 0; + // } + // lastToggle = millis(); + // } - if (millis() - prevSample >= SAMPLE_RATE) { - // this happens faster than control loop so that TBD smoothing function can be added - prevPosition = currentPosition; - currentPosition = encoderToPosition(encoderPosition); // get wall position (revolutions) from encoder reading - currentVelocity = 60000.0f * (currentPosition - prevPosition) / (float)(millis() - prevSample); // wall speed (rpm) - prevSample = millis(); - } + if (DMXSerial.noDataSince() < 5000) { + digitalWrite(R_EN_PIN, HIGH); + digitalWrite(L_EN_PIN, HIGH); - if (millis() - prevControl >= LOOP_RATE) { - // targetMotorVelocity = currentVelocity * MOTOR_RATIO; // begin with assumption we are going the right speed - double remainingDistance = targetPosition - currentPosition; - // double remainingTime; // projected running time assuming velocity doesn't change + if (micros() < prevSample) { + // micros timer rolled over, so reset sample timer: + // Serial.println("micros() roll-over"); + 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) { - // remainingTime = 1e6; - // } else { - // remainingTime = remainingDistance / currentVelocity; - // } - // if (abs(remainingDistance) < WALL_POSITION_TOLERANCE) { - // // 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); - // } - // } - // } + if (abs(remainingDistance) < WALL_POSITION_TOLERANCE) { + // we are close enough, stop moving: + targetMotorVelocity = 0.0f; + remainingTime = 0.0f; + } else { + targetMotorVelocity = sign(remainingDistance) * min(abs(remainingDistance) / SPEEDRAMP_DISTANCE * MOTOR_MAX_RPM, MOTOR_MAX_RPM); + } + // prevPWM = motorPWM; + prevSample = micros(); - // int motorPWM = Velocity_to_PWM(targetMotorVelocity); - // targetMotorVelocity += min((remainingDistance * 0.5) - targetMotorVelocity, MAX_MOTOR_ACCELERATION / LOOP_RATE / 1000.0f); - - // if (abs(remainingDistance) > WALL_POSITION_TOLERANCE) { - // if (((remainingDistance / 0.16 * 330) - targetMotorVelocity) > 0) { - // targetMotorVelocity = currentVelocity + min((remainingDistance / 0.16 * 330) - currentVelocity, MAX_MOTOR_ACCELERATION * (LOOP_RATE / 1000.0f)); - // } 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); + /* + Serial.print("sample updated:"); + Serial.print(" pos: ");Serial.print(currentPosition); + Serial.print(" vel: ");Serial.print(currentVelocity); + Serial.print(" error: ");Serial.print(remainingDistance); + Serial.println(); + */ } - - prevControl = millis(); - Serial.print(">ePos: "); - 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); + if (pid.Compute()) { + // newPWM = motorPWM; + motorPWM = Velocity_to_PWM(commandVelocity); + + // 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(); - showNewData(); - - // delay(5); - - // make sure DMX is still alive: - // // } else { - // // digitalWrite(R_EN_PIN, 0); - // // digitalWrite(L_EN_PIN, 0); - // // } + // react to Serial input (new target position): + // if (recvWithEndMarker()) {targetPosition = readNewTarget();} } +// 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() { if (digitalRead(ENCODER_B) == LOW) { encoderPosition--; // CCW @@ -260,40 +244,41 @@ double encoderToPosition(long encoderPosition) { return (encoderPosition / (float)ENCODER_PPR) / (float)ENCODER_RATIO; } -int Velocity_to_PWM(double motorVelocity) { - if (abs(motorVelocity) > 0) { - return min(round(MOTOR_MIN_PWM + ((255 - MOTOR_MIN_PWM) / (float)MOTOR_MAX_RPM) * abs(motorVelocity)), 255); +int Velocity_to_PWM(double commandVelocity) { + if (abs(commandVelocity) > 1) { + return min(round(MOTOR_MIN_PWM + ((255 - MOTOR_MIN_PWM) / (float)MOTOR_MAX_RPM) * abs(commandVelocity)), 255) * sign(commandVelocity); } return 0; } -void recvWithEndMarker() { - static byte ndx = 0; - char endMarker = '\n'; - char rc; +// bool recvWithEndMarker() { +// static byte ndx = 0; +// char endMarker = '\n'; +// char rc; - while (Serial.available() > 0 && newData == false) { - rc = Serial.read(); +// while (Serial.available() > 0 && newData == false) { +// rc = Serial.read(); - if (rc != endMarker) { - receivedChars[ndx] = rc; - ndx++; - if (ndx >= numChars) { - ndx = numChars - 1; - } - } - else { - receivedChars[ndx] = '\0'; // terminate the string - ndx = 0; - newData = true; - } - } -} +// if (rc != endMarker) { +// receivedChars[ndx] = rc; +// ndx++; +// if (ndx >= numChars) { +// ndx = numChars - 1; +// } +// } +// else { +// receivedChars[ndx] = '\0'; // terminate the string +// ndx = 0; +// newData = true; +// } +// } +// return newData; +// } -void showNewData() { - if (newData == true) { - Serial.print("This just in ... "); - Serial.println(receivedChars); - newData = false; - } -} \ No newline at end of file +// float readNewTarget() { +// Serial.print("New Target Position: "); +// Serial.println(receivedChars); +// delay(500); +// newData = false; +// return atof(receivedChars); +// } \ No newline at end of file