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

disabling with DMX

Browse Source
This commit is contained in:
Drake Marino 2024-10-30 22:17:04 -05:00
parent b92ed1a431
commit a2e8c50392
1 changed files with 92 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

172
src/main.cpp
View File

@ -115,10 +115,7 @@ void setup() {
void loop() { void loop() {
if (DMXSerial.noDataSince() < 5000) {
targetPosition = (float)(DMXSerial.read(DMX_CHANNEL) - 101) / 100.0f; // 0 is disabled. position values are 1-201, 101 is center, <101 is CW, >101 is CCW. 1 & 201 are 360 degrees
digitalWrite(R_EN_PIN, HIGH);
digitalWrite(L_EN_PIN, HIGH);
if (micros() < prevSample) { if (micros() < prevSample) {
// micros timer rolled over, so reset sample timer: // micros timer rolled over, so reset sample timer:
@ -126,92 +123,107 @@ void loop() {
prevSample = micros(); prevSample = micros();
currentPosition = encoderToPosition(encoderPosition); currentPosition = encoderToPosition(encoderPosition);
} else if (micros() - prevSample >= SAMPLE_RATE) { } else if (micros() - prevSample >= SAMPLE_RATE) {
// this can happen faster than control loop so that (TBD) smoothing function can be added if (DMXSerial.noDataSince() < 5000) {
prevPosition = currentPosition; int dmxVal = DMXSerial.read(DMX_CHANNEL);
double dt = (float (micros() - prevSample)) / (1.0e6f); // time since last sample (s) if (dmxVal != 0) {
currentPosition = encoderToPosition(encoderPosition); // get wall position (rev) from encoder reading targetPosition = (float)(dmxVal - 101) / 100.0f; // 0 is disabled. position values are 1-201, 101 is center, <101 is CW, >101 is CCW. 1 & 201 are 360 degrees
currentVelocity = 60.0f * (currentPosition - prevPosition) / dt; // wall speed (rpm) digitalWrite(R_EN_PIN, HIGH);
currentMotorVelocity = currentVelocity * MOTOR_RATIO; // motor speed (rpm), assuming no slipping digitalWrite(L_EN_PIN, HIGH);
remainingDistance = targetPosition - currentPosition; // position error (rev) // this can happen faster than control loop so that (TBD) smoothing function can be added
if (currentVelocity == 0) { prevPosition = currentPosition;
remainingTime = 1e6; double dt = (float (micros() - prevSample)) / (1.0e6f); // time since last sample (s)
} else { currentPosition = encoderToPosition(encoderPosition); // get wall position (rev) from encoder reading
remainingTime = remainingDistance/currentVelocity; 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 (abs(remainingDistance) < WALL_POSITION_TOLERANCE) { if (abs(remainingDistance) < WALL_POSITION_TOLERANCE) {
// we are close enough, stop moving: // we are close enough, stop moving:
targetMotorVelocity = 0.0f; targetMotorVelocity = 0.0f;
remainingTime = 0.0f; remainingTime = 0.0f;
} else { } else {
targetMotorVelocity = sign(remainingDistance) * min(abs(remainingDistance) / SPEEDRAMP_DISTANCE * MOTOR_MAX_RPM, MOTOR_MAX_RPM); targetMotorVelocity = sign(remainingDistance) * min(abs(remainingDistance) / SPEEDRAMP_DISTANCE * MOTOR_MAX_RPM, MOTOR_MAX_RPM);
} }
// prevPWM = motorPWM; // prevPWM = motorPWM;
prevSample = micros(); prevSample = micros();
/* /*
Serial.print("sample updated:"); Serial.print("sample updated:");
Serial.print(" pos: ");Serial.print(currentPosition); Serial.print(" pos: ");Serial.print(currentPosition);
Serial.print(" vel: ");Serial.print(currentVelocity); Serial.print(" vel: ");Serial.print(currentVelocity);
Serial.print(" error: ");Serial.print(remainingDistance); Serial.print(" error: ");Serial.print(remainingDistance);
Serial.println(); Serial.println();
*/ */
}
if (pid.Compute()) { if (pid.Compute()) {
// newPWM = motorPWM; // newPWM = motorPWM;
motorPWM = Velocity_to_PWM(commandVelocity); motorPWM = Velocity_to_PWM(commandVelocity);
// update motor controller command: // update motor controller command:
if (abs(motorPWM) < MOTOR_MIN_PWM) { if (abs(motorPWM) < MOTOR_MIN_PWM) {
// Serial.println("PWM < MIN"); // Serial.println("PWM < MIN");
digitalWrite(RPWM_PIN, LOW); digitalWrite(RPWM_PIN, LOW);
digitalWrite(RLED_PIN, LOW); digitalWrite(RLED_PIN, LOW);
digitalWrite(LPWM_PIN, LOW); digitalWrite(LPWM_PIN, LOW);
digitalWrite(LLED_PIN, LOW); digitalWrite(LLED_PIN, LOW);
} else if (motorPWM > 0) { } else if (motorPWM > 0) {
// Serial.println("PWM > 0"); // Serial.println("PWM > 0");
digitalWrite(RPWM_PIN, LOW); digitalWrite(RPWM_PIN, LOW);
digitalWrite(RLED_PIN, LOW); digitalWrite(RLED_PIN, LOW);
pwmWrite(LPWM_PIN, motorPWM); pwmWrite(LPWM_PIN, motorPWM);
pwmWrite(LLED_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);
}
} else { } else {
// Serial.println("Else"); digitalWrite(R_EN_PIN, LOW);
digitalWrite(L_EN_PIN, LOW);
digitalWrite(LPWM_PIN, LOW); digitalWrite(LPWM_PIN, LOW);
digitalWrite(LLED_PIN, LOW); digitalWrite(RPWM_PIN, LOW);
pwmWrite(RPWM_PIN, -motorPWM);
pwmWrite(RLED_PIN, -motorPWM);
} }
} else {
// generate some debug output: digitalWrite(R_EN_PIN, LOW);
// Serial.print(">time: "); digitalWrite(L_EN_PIN, LOW);
// Serial.print(millis()); digitalWrite(LPWM_PIN, LOW);
// Serial.print(", ePos: "); digitalWrite(RPWM_PIN, LOW);
// 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);
}
// react to Serial input (new target position): // react to Serial input (new target position):
// if (recvWithEndMarker()) {targetPosition = readNewTarget();} // if (recvWithEndMarker()) {targetPosition = readNewTarget();}