disabling with DMX
This commit is contained in:
parent
b92ed1a431
commit
a2e8c50392
172
src/main.cpp
172
src/main.cpp
@ -115,10 +115,7 @@ void setup() {
|
||||
|
||||
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) {
|
||||
// micros timer rolled over, so reset sample timer:
|
||||
@ -126,92 +123,107 @@ void loop() {
|
||||
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 (DMXSerial.noDataSince() < 5000) {
|
||||
int dmxVal = DMXSerial.read(DMX_CHANNEL);
|
||||
if (dmxVal != 0) {
|
||||
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
|
||||
digitalWrite(R_EN_PIN, HIGH);
|
||||
digitalWrite(L_EN_PIN, HIGH);
|
||||
// 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 (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);
|
||||
}
|
||||
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();
|
||||
// prevPWM = motorPWM;
|
||||
prevSample = micros();
|
||||
|
||||
/*
|
||||
Serial.print("sample updated:");
|
||||
Serial.print(" pos: ");Serial.print(currentPosition);
|
||||
Serial.print(" vel: ");Serial.print(currentVelocity);
|
||||
Serial.print(" error: ");Serial.print(remainingDistance);
|
||||
Serial.println();
|
||||
*/
|
||||
}
|
||||
/*
|
||||
Serial.print("sample updated:");
|
||||
Serial.print(" pos: ");Serial.print(currentPosition);
|
||||
Serial.print(" vel: ");Serial.print(currentVelocity);
|
||||
Serial.print(" error: ");Serial.print(remainingDistance);
|
||||
Serial.println();
|
||||
*/
|
||||
|
||||
if (pid.Compute()) {
|
||||
// newPWM = motorPWM;
|
||||
if (pid.Compute()) {
|
||||
// newPWM = motorPWM;
|
||||
|
||||
motorPWM = Velocity_to_PWM(commandVelocity);
|
||||
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);
|
||||
// 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);
|
||||
}
|
||||
} else {
|
||||
digitalWrite(R_EN_PIN, LOW);
|
||||
digitalWrite(L_EN_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);
|
||||
} else {
|
||||
digitalWrite(R_EN_PIN, LOW);
|
||||
digitalWrite(L_EN_PIN, LOW);
|
||||
digitalWrite(LPWM_PIN, LOW);
|
||||
digitalWrite(RPWM_PIN, LOW);
|
||||
}
|
||||
}
|
||||
// make sure DMX is still alive:
|
||||
} else {
|
||||
digitalWrite(R_EN_PIN, LOW);
|
||||
digitalWrite(L_EN_PIN, LOW);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// react to Serial input (new target position):
|
||||
// if (recvWithEndMarker()) {targetPosition = readNewTarget();}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user