remaining dist and brake time version
This commit is contained in:
parent
56bf063682
commit
3686061e3a
369
src/main.cpp
369
src/main.cpp
@ -1,120 +1,299 @@
|
|||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
#include <DMXSerial.h>
|
//#include <DMXSerial.h>
|
||||||
#include <PID_v1.h>
|
#include <PID_v1.h>
|
||||||
#include <PWM.h>
|
#include <PWM.h>
|
||||||
|
|
||||||
|
// 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
|
// Encoder p/n: e38s6g5-600b-g24n
|
||||||
#define RPWM_PIN 9
|
|
||||||
#define LPWM_PIN 10
|
|
||||||
#define L_EN_PIN 11
|
|
||||||
#define R_EN_PIN 12
|
|
||||||
#define ENCODER_A 2
|
#define ENCODER_A 2
|
||||||
#define ENCODER_B 3
|
#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;
|
const int DMX_CHANNEL = 1;
|
||||||
|
|
||||||
double pidGain = 0.01;
|
long encoderPosition = 0; // raw count from encoder
|
||||||
double currentPosition = 0;
|
double maxWallVelocity = MOTOR_MAX_RPM / MOTOR_RATIO;
|
||||||
double targetPosition = 0;
|
double currentPosition = 0; // wall (revolutions)
|
||||||
// int positionDif = 0;
|
double prevPosition = 0; // wall (revolutions)
|
||||||
double pidPWM = 0;
|
double targetPosition = 0; // wall (revolutions)
|
||||||
double motorPWM = 0;
|
double currentVelocity = 0; // wall velocity (rpm)
|
||||||
double lastSwitch = 0;
|
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() {
|
void updateEncoder() {
|
||||||
if (digitalRead(ENCODER_B) == LOW) {
|
if (digitalRead(ENCODER_B) == LOW) {
|
||||||
currentPosition++; // CCW
|
encoderPosition--; // CCW
|
||||||
} else {
|
} else {
|
||||||
currentPosition--; // CW
|
encoderPosition++; // CW
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void setup() {
|
double encoderToPosition(long encoderPosition) {
|
||||||
Serial.begin(115200);
|
return (encoderPosition / (float)ENCODER_PPR) / (float)ENCODER_RATIO;
|
||||||
|
|
||||||
// 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();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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() {
|
void recvWithEndMarker() {
|
||||||
if ((millis() - lastSwitch) > 5000) {
|
static byte ndx = 0;
|
||||||
if (targetPosition == 0) {
|
char endMarker = '\n';
|
||||||
targetPosition = 3000;
|
char rc;
|
||||||
} else {
|
|
||||||
targetPosition = 0;
|
while (Serial.available() > 0 && newData == false) {
|
||||||
|
rc = Serial.read();
|
||||||
|
|
||||||
|
if (rc != endMarker) {
|
||||||
|
receivedChars[ndx] = rc;
|
||||||
|
ndx++;
|
||||||
|
if (ndx >= numChars) {
|
||||||
|
ndx = numChars - 1;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
lastSwitch = millis();
|
else {
|
||||||
}
|
receivedChars[ndx] = '\0'; // terminate the string
|
||||||
// Calculate how long no data bucket was received
|
ndx = 0;
|
||||||
// if (DMXSerial.noDataSince() < 5000) {
|
newData = true;
|
||||||
digitalWrite(R_EN_PIN, 1);
|
}
|
||||||
digitalWrite(L_EN_PIN, 1);
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// read recent DMX values and set pwm levels
|
void showNewData() {
|
||||||
// pidGain = 0.0001 + (0.002 * (DMXSerial.read(DMX_CHANNEL + 1) / 255));
|
if (newData == true) {
|
||||||
// float kP = 10 * (DMXSerial.read(DMX_CHANNEL + 2) / 255);
|
Serial.print("This just in ... ");
|
||||||
// float kI = 10 * (DMXSerial.read(DMX_CHANNEL + 3) / 255);
|
Serial.println(receivedChars);
|
||||||
// float kD = 10 * (DMXSerial.read(DMX_CHANNEL + 4) / 255);
|
newData = false;
|
||||||
// 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);
|
|
||||||
// }
|
|
||||||
}
|
}
|
||||||
Loading…
Reference in New Issue
Block a user