Skip to content

Commit

Permalink
Merge pull request #318 from krkeegan/elapsed_time_velocity
Browse files Browse the repository at this point in the history 
Improvements For Low RPM Speed Calculation
  • Loading branch information
@BarbourSmith
BarbourSmith authored Oct 12, 2017
2 parents 0c8adf3 + bcfafca commit cdeede3
Show file tree
Hide file tree
Showing 4 changed files with 75 additions and 69 deletions.
8 changes: 2 additions & 6 deletions cnc_ctrl_v1/CNC_Functions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1232,9 +1232,7 @@ void PIDTestVelocity(Axis* axis, const float start, const float stop, const floa
double startTime;
double print = micros();
double current = micros();
float lastPosition = axis->motorGearboxEncoder.encoder.read();
float error;
float distMoved;
float reportedSpeed;
float span = stop - start;
float speed;
Expand All @@ -1253,10 +1251,8 @@ void PIDTestVelocity(Axis* axis, const float start, const float stop, const floa
while (startTime + 2000000 > current){
if (current - print > 20000){
// Calculate and log error on same frequency as PID interrupt
distMoved = axis->motorGearboxEncoder.encoder.read() - lastPosition;
reportedSpeed= (7364.0*distMoved)/float(current - print); //6*10^7 us per minute, 8148 steps per revolution
lastPosition = axis->motorGearboxEncoder.encoder.read();
error = (-1.0 * reportedSpeed) - speed;
reportedSpeed= axis->motorGearboxEncoder.cachedSpeed();
error = reportedSpeed - speed;
print = current;
Serial.println(error);
}
Expand Down
27 changes: 26 additions & 1 deletion cnc_ctrl_v1/Encoder.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,8 @@ typedef struct {
IO_REG_TYPE pin2_bitmask;
uint8_t state;
int32_t position;
int32_t elapsedTime;
int32_t lastTime;
} Encoder_internal_state_t;

class Encoder
Expand Down Expand Up @@ -112,6 +114,17 @@ class Encoder
interrupts();
return ret;
}
inline int32_t elapsedTime() {
if (interrupts_in_use < 2) {
noInterrupts();
update(&encoder);
} else {
noInterrupts();
}
int32_t ret = encoder.elapsedTime;
interrupts();
return ret;
}
inline void write(int32_t p) {
noInterrupts();
encoder.position = p;
Expand Down Expand Up @@ -183,7 +196,11 @@ class Encoder

private:
static void update(Encoder_internal_state_t *arg) {
#if defined(__AVR__)
#if defined(__AVR_DISABLED__)
// Changed from __AVR__ to force C code below, until someone has
// the chance to write the elapsedTime feature into assembly
// code. The time difference between the two options is
// minimal at best.
// The compiler believes this is just 1 line of code, so
// it will inline this function into each interrupt
// handler. That's a tiny bit faster, but grows the code.
Expand Down Expand Up @@ -277,15 +294,23 @@ class Encoder
switch (state) {
case 1: case 7: case 8: case 14:
arg->position++;
arg->elapsedTime = (micros() - arg->lastTime);
arg->lastTime = micros();
return;
case 2: case 4: case 11: case 13:
arg->position--;
arg->elapsedTime = (arg->lastTime - micros());
arg->lastTime = micros();
return;
case 3: case 12:
arg->position += 2;
arg->elapsedTime = (micros() - arg->lastTime) >> 1;
arg->lastTime = micros();
return;
case 6: case 9:
arg->position -= 2;
arg->elapsedTime = (arg->lastTime - micros()) >> 1;
arg->lastTime = micros();
return;
}
#endif
Expand Down
99 changes: 41 additions & 58 deletions cnc_ctrl_v1/MotorGearboxEncoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,58 +60,10 @@ void MotorGearboxEncoder::computePID(){
/*
Recompute the speed control PID loop and command the motor to move.
*/
_currentSpeed = computeSpeed();

/*if (millis() < 8000){
_targetSpeed = 0;
}
else if (millis() < 12000){
_targetSpeed = 10;
}
else if (millis() < 18000){
_targetSpeed = 0;
}
else if(millis() < 24000){
_targetSpeed = float((millis() - 18000))/400.0;
}
else if (millis() < 32000){
_targetSpeed = 0;
}
else if (millis() < 40000){
_targetSpeed = 10;
}
else if (millis() < 48000){
_targetSpeed = 0;
}
else if (millis() < 56000){
_targetSpeed = -10;
}
else if (millis() < 64000){
_targetSpeed = 0;
}
else if (millis() < 72000){
_targetSpeed = 10;
}
else if (millis() < 80000){
_targetSpeed = 0;
}
else{
_targetSpeed = 0;
}*/

// Between these speeds the motor is incapable of turning and it only
// causes the Iterm in the PID calculation to wind up
if (abs(_targetSpeed) <= _minimumRPM) _targetSpeed = 0;
_currentSpeed = _computeSpeed();

_PIDController.Compute();

/*if(_motorName[0] == 'R'){
//Serial.print(_currentSpeed);
//Serial.print(" ");
Serial.println(_targetSpeed);
}*/

//motor.attach();

motor.write(_pidOutput);
}

Expand Down Expand Up @@ -162,24 +114,55 @@ void MotorGearboxEncoder::setEncoderResolution(float resolution){

}

float MotorGearboxEncoder::computeSpeed(){
float MotorGearboxEncoder::_computeSpeed(){
/*
Returns the motors speed in RPM since the last time this function was called
should only be called by the PID process otherwise we are calculating the
distance moved over a varying amount of time.
*/
double timeElapsed = micros() - _lastTimeStamp;

float distMoved = encoder.read() - _lastPosition; //_runningAverage(encoder.read() - _lastPosition); //because of quantization noise it helps to average these
float currentPosition = encoder.read();
float currentMicros = micros();

float distMoved = currentPosition - _lastPosition;
if (distMoved > 3 || distMoved < -3){

// This dampens some of the effects of quantization without having
// a big effect on other changes
float saveDistMoved = distMoved;
if (distMoved - _lastDistMoved <= -1){ distMoved + .5;}
else if (distMoved - _lastDistMoved >= 1){distMoved - .5;}
_lastDistMoved = saveDistMoved;

unsigned long timeElapsed = currentMicros - _lastTimeStamp;
//Compute the speed in RPM
_RPM = (_encoderStepsToRPMScaleFactor*distMoved)/float(timeElapsed);

//Compute the speed in RPM
float RPM = (_encoderStepsToRPMScaleFactor*distMoved)/float(timeElapsed);
}
else {
float elapsedTime = encoder.elapsedTime();

_RPM = 0 ;
if (elapsedTime != 0){
_RPM = _encoderStepsToRPMScaleFactor / elapsedTime;
}
}
_RPM = _RPM * -1.0;

//Store values for next time
_lastTimeStamp = micros();
_lastPosition = encoder.read();
_lastTimeStamp = currentMicros;
_lastPosition = currentPosition;

return -1.0*RPM;
return _RPM;
}

float MotorGearboxEncoder::cachedSpeed(){
/*
Returns the last result of computeSpeed
*/
return _RPM;
}

void MotorGearboxEncoder::setName(const char& newName){
Expand Down
10 changes: 6 additions & 4 deletions cnc_ctrl_v1/MotorGearboxEncoder.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
MotorGearboxEncoder(const int& pwmPin, const int& directionPin1, const int& directionPin2, const int& encoderPin1, const int& encoderPin2);
Encoder encoder;
Motor motor;
float computeSpeed();
float cachedSpeed();
void write(const float& speed);
void computePID();
void setName(const char& newName);
Expand All @@ -41,14 +41,16 @@
private:
double _targetSpeed;
double _currentSpeed;
double _lastPosition;
double _lastTimeStamp;
volatile double _lastPosition;
volatile double _lastTimeStamp;
float _lastDistMoved;
float _RPM;
char _motorName;
double _pidOutput;
PID _PIDController;
double _Kp=0, _Ki=0, _Kd=0;
float _encoderStepsToRPMScaleFactor = 7364.0; //6*10^7 us per minute divided by 8148 steps per revolution
float _minimumRPM = 0.5;
float _computeSpeed();
};

#endif

0 comments on commit cdeede3

Please sign in to comment.