51 lines
1.0 KiB
C++
51 lines
1.0 KiB
C++
#include <AS5600.h>
|
|
#include <Wire.h>
|
|
#include "spindle.h"
|
|
#include "constants.h"
|
|
|
|
AS5600 spindle(&Wire);
|
|
|
|
bool setupSpindle() {
|
|
#if DEBUG_DISABLE_SPINDLE
|
|
Serial.println("Skipping spindle");
|
|
return true;
|
|
#endif
|
|
|
|
if (!spindle.begin()) {
|
|
Serial.println("Could not initialize spindle encoder");
|
|
return false;
|
|
}
|
|
|
|
if (!validateSpindle()) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool validateSpindle() {
|
|
if (!spindle.detectMagnet()) {
|
|
Serial.println("Could not detect magnet");
|
|
return false;
|
|
} else if (spindle.magnetTooWeak()) {
|
|
// NOTE: We get this message all the time even though spindle position reading seems fine
|
|
// and we can't bring magnet closer to the IC, so we'll ignore this for now
|
|
Serial.println("Magnet too weak");
|
|
// return false;
|
|
} else if (spindle.magnetTooStrong()) {
|
|
Serial.println("Magnet too strong");
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
uint16_t getSpindlePosition() {
|
|
return spindle.readAngle();
|
|
}
|
|
|
|
void spindleDemo() {
|
|
validateSpindle();
|
|
Serial.println(getSpindlePosition());
|
|
}
|