276 lines
10 KiB
C++
276 lines
10 KiB
C++
#include <Arduino.h>
|
|
#include <DMXSerial.h>
|
|
#include <PID_v1.h>
|
|
#include <../lib/PWM/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
|
|
|
|
// 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 600 // pulses per revolution
|
|
#define ENCODER_RATIO 2.0f // number of times encoder spins per revolution of wall
|
|
#define MOTOR_RATIO 10.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 20 // 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 SPEEDRAMP_DISTANCE 0.3f // range target velocity should be proportional to error (10 degrees = 0.0278 rev)
|
|
#define WALL_POSITION_TOLERANCE 0.0056f // acceptable error in wall position (2 degrees = 0.0056 rev)
|
|
|
|
#define SAMPLE_RATE 10000 // min time (us) between velocity calculations
|
|
#define LOOP_RATE 10 // min time (ms) between control adjustments. ** Must be >= SAMPLE_RATE (converted to us) **
|
|
|
|
#define DMX_CHANNEL 2 // DMX Channel to read
|
|
|
|
// wall state variables:
|
|
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)
|
|
|
|
// motor control variables:
|
|
double currentMotorVelocity = 0; // motor velocity (rpm)
|
|
double targetMotorVelocity = 0; // motor velocity (rpm)
|
|
|
|
unsigned long prevSample = 0; // time tracker for sensor sampling rate (us)
|
|
unsigned long prevControl = 0; // time tracker for control loop (ms)
|
|
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
|
|
double motorPWM = 0; // motor PWM command level (-255/+255)
|
|
double commandVelocity = 0; // PID commanded motor velocity (RPM)
|
|
|
|
//debug variables:
|
|
// int prevPWM = 0; // motor command from previous loop
|
|
// int newPWM = 0; // updated motor command
|
|
// int dummyTarget = 0;
|
|
// long lastToggle = 0;
|
|
|
|
// 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
|
|
// bool recvWithEndMarker(); // read Serial input
|
|
// float readNewTarget(); // convert Serial input to new target position
|
|
int sign(float); // get sign of a value: (returns +1 or -1)
|
|
|
|
//PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, ControllerDirection)
|
|
PID pid = PID(¤tMotorVelocity, &commandVelocity, &targetMotorVelocity, 255.0f/330.0f, 0.0, 0.0, DIRECT);
|
|
// PID SetSampleTime((int) LOOP_RATE);
|
|
|
|
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);
|
|
SetPinFrequencySafe(10, 10000);
|
|
digitalWrite(LPWM_PIN, LOW);
|
|
digitalWrite(RPWM_PIN, LOW);
|
|
digitalWrite(L_EN_PIN, HIGH);
|
|
digitalWrite(R_EN_PIN, HIGH);
|
|
|
|
pid.SetSampleTime(LOOP_RATE);
|
|
pid.SetMode(AUTOMATIC);
|
|
pid.SetOutputLimits(-330, 330);
|
|
|
|
// initialize loop timers
|
|
prevSample = millis();
|
|
prevControl = millis();
|
|
}
|
|
|
|
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:
|
|
// Serial.println("micros() roll-over");
|
|
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 (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();
|
|
|
|
/*
|
|
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;
|
|
|
|
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);
|
|
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);
|
|
}
|
|
// 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();}
|
|
}
|
|
|
|
// return +1 if positive or -1 if negative
|
|
int sign(float val) {
|
|
// return +1 if positive or -1 if negative
|
|
if (val >= 0) {return 1;} else {return -1;}
|
|
}
|
|
|
|
// got an interrupt on ENCODER_A, so update encoder position
|
|
void updateEncoder() {
|
|
if (digitalRead(ENCODER_B) == LOW) {
|
|
encoderPosition--; // CCW
|
|
} else {
|
|
encoderPosition++; // CW
|
|
}
|
|
}
|
|
|
|
double encoderToPosition(long encoderPosition) {
|
|
return (encoderPosition / (float)ENCODER_PPR) / (float)ENCODER_RATIO;
|
|
}
|
|
|
|
int Velocity_to_PWM(double commandVelocity) {
|
|
if (abs(commandVelocity) > 1) {
|
|
return min(round(MOTOR_MIN_PWM + ((255 - MOTOR_MIN_PWM) / (float)MOTOR_MAX_RPM) * abs(commandVelocity)), 255) * sign(commandVelocity);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
// bool 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;
|
|
// }
|
|
// }
|
|
// else {
|
|
// receivedChars[ndx] = '\0'; // terminate the string
|
|
// ndx = 0;
|
|
// newData = true;
|
|
// }
|
|
// }
|
|
// return newData;
|
|
// }
|
|
|
|
// float readNewTarget() {
|
|
// Serial.print("New Target Position: ");
|
|
// Serial.println(receivedChars);
|
|
// delay(500);
|
|
// newData = false;
|
|
// return atof(receivedChars);
|
|
// }
|