diff --git a/src/main.cpp b/src/main.cpp index 5654bb1..16e8e23 100644 --- a/src/main.cpp +++ b/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();}