diff --git a/src/main.cpp b/src/main.cpp index 50a2d98..f9aeccb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,120 +1,299 @@ #include -#include +//#include #include #include +// Motor Driver: IBT-2 (BTS7960) +#define LPWM_PIN 9 // LPWM for CCW motion (postive) +#define RPWM_PIN 10 // RPWM for CW motion (negative) +#define L_EN_PIN 11 // Both enable pins must be high to drive motor +#define R_EN_PIN 12 // Both enable pins must be high to drive motor -// #define SENSOR_PIN A0 -#define RPWM_PIN 9 -#define LPWM_PIN 10 -#define L_EN_PIN 11 -#define R_EN_PIN 12 +// Encoder p/n: e38s6g5-600b-g24n #define ENCODER_A 2 #define ENCODER_B 3 +// LED indicators to follow motor pwm output +#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 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 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 ** + const int DMX_CHANNEL = 1; -double pidGain = 0.01; -double currentPosition = 0; -double targetPosition = 0; -// int positionDif = 0; -double pidPWM = 0; -double motorPWM = 0; -double lastSwitch = 0; +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) + +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) + +// Serial Input processor: +const byte numChars = 32; +char receivedChars[numChars]; // an array to store the received data +boolean newData = false; + +// Function Prototypes: +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 + +PID pid = PID(¤tPosition, &targetMotorVelocity, &targetPosition, 600, 0.25, 5.0, DIRECT); + +void setup() { + Serial.begin(115200); + + // Configure DMX shield (MAX485 chipset) + // DMXSerial.init(DMXReceiver); + // DMXSerial.write(DMX_CHANNEL, 0); + + // Initialize GPIO pins: + pinMode(RPWM_PIN, OUTPUT); + pinMode(LPWM_PIN, OUTPUT); + pinMode(L_EN_PIN, OUTPUT); + pinMode(R_EN_PIN, OUTPUT); + pinMode(LLED_PIN, OUTPUT); + pinMode(RLED_PIN, OUTPUT); + pinMode(ENCODER_A, INPUT_PULLUP); + pinMode(ENCODER_B, INPUT_PULLUP); + + // Start encoder: + attachInterrupt(digitalPinToInterrupt(ENCODER_A), updateEncoder, RISING); + + // Start the Motor Driver - Set Frequency, start both PWM low, Enable both sides: + InitTimersSafe(); + SetPinFrequencySafe(9, 10000); + digitalWrite(LPWM_PIN, LOW); + digitalWrite(RPWM_PIN, LOW); + digitalWrite(L_EN_PIN, HIGH); + digitalWrite(R_EN_PIN, HIGH); + + pid.SetSampleTime(10); + pid.SetMode(AUTOMATIC); + pid.SetOutputLimits(-330, 330); + + // initialize loop timers + prevSample = millis(); + prevControl = millis(); +} + +void loop() { + if (millis() < prevControl) { + // millis timer rolled over, so reset sample and control timers: + prevSample = millis(); + prevControl = millis(); + currentPosition = encoderToPosition(encoderPosition); + } + + 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 (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 (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); + // } + // } + // } -PID pid(¤tPosition, &pidPWM, &targetPosition, 0.2, 0.0, 0.1, DIRECT); + // 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); + } + + 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); + + } + + recvWithEndMarker(); + showNewData(); + + // delay(5); + + // make sure DMX is still alive: + // // } else { + // // digitalWrite(R_EN_PIN, 0); + // // digitalWrite(L_EN_PIN, 0); + // // } +} void updateEncoder() { if (digitalRead(ENCODER_B) == LOW) { - currentPosition++; // CCW + encoderPosition--; // CCW } else { - currentPosition--; // CW + encoderPosition++; // CW } } -void setup() { - Serial.begin(115200); - - // DMXSerial.init(DMXReceiver); - // DMXSerial.write(DMX_CHANNEL, 0); - - pinMode(RPWM_PIN, OUTPUT); - pinMode(LPWM_PIN, OUTPUT); - pinMode(L_EN_PIN, OUTPUT); - pinMode(R_EN_PIN, OUTPUT); - pinMode(ENCODER_A, INPUT_PULLUP); - pinMode(ENCODER_B, INPUT_PULLUP); - - attachInterrupt(digitalPinToInterrupt(ENCODER_A), updateEncoder, RISING); - - InitTimersSafe(); - SetPinFrequencySafe(9, 10000); - - pid.SetMode(AUTOMATIC); - pid.SetSampleTime(50); - pid.SetOutputLimits(-255, 255); - - pinMode(5, OUTPUT); - pinMode(6, OUTPUT); - digitalWrite(LPWM_PIN, HIGH); - digitalWrite(RPWM_PIN, HIGH); - lastSwitch = millis(); +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); + } + return 0; +} -void loop() { - if ((millis() - lastSwitch) > 5000) { - if (targetPosition == 0) { - targetPosition = 3000; - } else { - targetPosition = 0; +void recvWithEndMarker() { + static byte ndx = 0; + char endMarker = '\n'; + char rc; + + while (Serial.available() > 0 && newData == false) { + rc = Serial.read(); + + if (rc != endMarker) { + receivedChars[ndx] = rc; + ndx++; + if (ndx >= numChars) { + ndx = numChars - 1; + } } - lastSwitch = millis(); - } - // Calculate how long no data bucket was received - // if (DMXSerial.noDataSince() < 5000) { - digitalWrite(R_EN_PIN, 1); - digitalWrite(L_EN_PIN, 1); + else { + receivedChars[ndx] = '\0'; // terminate the string + ndx = 0; + newData = true; + } + } +} - // read recent DMX values and set pwm levels - // pidGain = 0.0001 + (0.002 * (DMXSerial.read(DMX_CHANNEL + 1) / 255)); - // float kP = 10 * (DMXSerial.read(DMX_CHANNEL + 2) / 255); - // float kI = 10 * (DMXSerial.read(DMX_CHANNEL + 3) / 255); - // float kD = 10 * (DMXSerial.read(DMX_CHANNEL + 4) / 255); - // pid.SetTunings(kP, kI, kD); - - // targetPosition = (DMXSerial.read(DMX_CHANNEL) - 100) * 6; // 0-200. 100 is center, <100 is CW, >100 is CCW. 0 & 200 are 360 degrees - - pid.Compute(); - // motorPWM += (pidPWM - motorPWM) * pidGain; - motorPWM = pidPWM; - - Serial.print("current: "); - Serial.print(currentPosition); - Serial.print(", target: "); - Serial.print(targetPosition); - Serial.print(", PID: "); - Serial.print(pidPWM); - Serial.print(", Motor: "); - Serial.println(motorPWM); - - if (motorPWM > 0) { - pwmWrite(RPWM_PIN, 0); - pwmWrite(LPWM_PIN, motorPWM); - - analogWrite(5, 0); - analogWrite(6, motorPWM); - } else { - pwmWrite(LPWM_PIN, 0); - pwmWrite(RPWM_PIN, -motorPWM); - - analogWrite(6, 0); - analogWrite(5, -motorPWM); - } - - // } else { - // digitalWrite(R_EN_PIN, 0); - // digitalWrite(L_EN_PIN, 0); - // } +void showNewData() { + if (newData == true) { + Serial.print("This just in ... "); + Serial.println(receivedChars); + newData = false; + } } \ No newline at end of file