/// <summary>
/// Creates a new <see cref="L6470"/> instance.
/// </summary>
/// <param name="ChipSelectPin">The number of the chip select pin for this device,
/// using the BCM numbering scheme.</param>
/// <param name="ResetPin">The number of the reset pin for this device, using the
/// BCM numbering scheme.</param>
/// <param name="StepAngle">The angle that a single motor step moves the motor shaft,
/// in degrees</param>
/// <param name="MaxCurrent">The max current the motor can sustain, per coil, in amps.</param>
/// <remarks>
/// As a quick note, the L6470's SPI spec actually *requires* that the chip select be
/// put back to high after every byte, then pulled low again to read the next byte.
/// I don't know why it works this way, but it does. The getters and setters will look
/// different from the AMT22, where the chip select stays low for the entire exchange...
/// but that's how it is!
/// </remarks>
public L6470(byte ChipSelectPin, byte ResetPin, double StepAngle, double MaxCurrent)
{
Spi = new SpiDevice(ChipSelectPin, 1000000, SpiMode.Mode3, 0, 0, 0, 0);
this.StepAngle = StepAngle;
this.MaxCurrent = MaxCurrent;
// Reset the driver
pinMode(ResetPin, PinMode.Output);
digitalWrite(ResetPin, PinState.Low);
Thread.Sleep(1);
digitalWrite(ResetPin, PinState.High);
Thread.Sleep(1);
// Get the current config
ushort config = GetParam_16Bit(Register.Configuration);
L6470Status status = GetStatus();
// Set the overcurrent threshold
byte overcurrentTheshold = GetClosestOvercurrentThreshold(MaxCurrent);
SetParam_8Bit(Register.OvercurrentThreshold, overcurrentTheshold);
// Default to 16 microsteps: SYNC_EN = 0, SYNC_SEL = 0, STEP_SEL = 4
byte stepMode = (byte)(MicrostepMode._16PerStep);
SetParam_8Bit(Register.StepMode, stepMode);
// Default to a max speed of as high as it goes, I guess...
ushort maxSpeed = 0x03FF;
SetParam_16Bit(Register.MaximumSpeed, maxSpeed);
// Set the full-step speed (the threshold at which it disables microstepping)
// to a default of 300 RPM. That seems like a lot so it's probably a good
// starting point.
ushort fullStepSpeed = GetFormattedFullStepSpeed(300.0);
SetParam_16Bit(Register.FullStepSpeed, fullStepSpeed);
// Set the acceleration and deceleration to 500 RPM/sec.
ushort acceleration = GetFormattedAcceleration(500.0);
SetParam_16Bit(Register.Acceleration, acceleration);
SetParam_16Bit(Register.Deceleration, acceleration);
// Set the config register to the following defaults:
// OSC_SEL and EXT_CLK = 0000; 16 MHz internal clock, no output
// SW_MODE = 0; hard stop when the kill switch is thrown
// EN_VSCOMP = 1; enabled
// OC_SD = 1; enabled
// POW_SR = 11; 530 V/us for maximum torque
// F_PWM_INT and F_PWM_DEC = 000111; 62.5 kHz PWM output
config = 0;
//config |= (1 << 5); // Set EN_VSCOMP to 1
config |= (1 << 7); // Set OC_SD to 1
config |= (0b11 << 8); // Set POW_SR to 530 V/us
config |= ((byte)PwmFrequency._31_250 << 10);
SetParam_16Bit(Register.Configuration, config);
}
/// <summary>
/// Immediately stops the motor, ignoring the deceleration curve,
/// and disenages the coils. This is like an "EMERGENCY STOP!" function.
/// This will block until the driver reports that the stop has finished
/// and the system is now idle.
/// </summary>
public void HardHiZ()
{
byte[] buffer = { 0b10101000 };
Spi.TransferData(buffer);
// Wait for the HiZ to finish
L6470Status status = GetStatus();
while (status.IsBusy || status.BridgesActive)
{
Thread.Sleep(10);
status = GetStatus();
}
}
/// <summary>
/// Immediately stops the motor, ignoring the deceleration curve,
/// but keeps the coils engaged (holding). This will block until the driver
/// reports that the stop has finished and the system is now idle.
/// </summary>
public void HardStop()
{
byte[] buffer = { 0b10111000 };
Spi.TransferData(buffer);
// Wait for the stop finish
L6470Status status = GetStatus();
while (status.IsBusy)
{
Thread.Sleep(10);
status = GetStatus();
}
}
public void RefreshStatus_Click(object sender, RoutedEventArgs e)
{
int position = Encoder.GetPosition();
ShaftPositionBox.Text = position.ToString();
L6470Status status = Driver.GetStatus();
CoilAStalledBox.Text = (status.BridgeAStalled ? "YES" : "");
CoilBStalledBox.Text = (status.BridgeBStalled ? "YES" : "");
OvercurrentBox.Text = (status.OvercurrentDetected ? "YES" : "");
ThermalShutdownBox.Text = (status.ThermalShutdownTriggered ? "YES" : "");
ThermalWarningBox.Text = (status.ThermalWarningTriggered ? "YES" : "");
UndervoltageBox.Text = (status.UndervoltageDetected ? "YES" : "");
UnknownCommandBox.Text = (status.ReceivedUnknownCommand ? "YES" : "");
LastCommandFailedBox.Text = (status.LastCommandFailed ? "YES" : "");
MotorStateBox.Text = status.MotorState.ToString();
DirectionBox.Text = status.Direction.ToString();
KillSwitchTriggerBox.Text = (status.KillSwitchTriggered ? "YES" : "");
KillSwitchActiveBox.Text = (status.KillSwitchActive ? "YES" : "");
IsBusyBox.Text = (status.IsBusy ? "YES" : "");
HiZBox.Text = (status.BridgesActive ? "YES" : "");
}
private void CheckL6470State(L6470Status Status)
{
if (Status.BridgeAStalled)
{
throw new Exception($"Focus motor bridge A is stalled.");
}
if (Status.BridgeBStalled)
{
throw new Exception($"Focus motor bridge B is stalled.");
}
if (Status.OvercurrentDetected)
{
throw new Exception($"Focus motor exceeded its current limit.");
}
if (Status.ThermalShutdownTriggered)
{
throw new Exception($"Focus motor driver overheated.");
}
if (Status.UndervoltageDetected)
{
throw new Exception($"Focus motor fell below its motor voltage threshold.");
}
}
private void MovementLoop()
{
FocusMoveResult result;
while (!StopMove)
{
// Check the current status and report any failures
L6470Status status = Driver.GetStatus();
try
{
CheckL6470State(status);
}
catch (Exception ex)
{
Driver.SoftHiZ();
MoveTask = null;
result = new FocusMoveResult(false, ex.Message);
MoveFinished?.Invoke(this, result);
return;
}
// Get the current position and distance to the target
int currentPosition = Encoder.GetPosition();
int delta = Math.Abs(currentPosition - DesiredPosition);
// See which direction we're supposed to go, or if we're close enough
MotorAction action = MotorAction.Stop;
if (delta < TargetThreshold)
{
action = MotorAction.Stop;
}
else if (DesiredPosition > currentPosition)
{
action = MotorAction.Forward;
delta = Math.Min(delta, UpperEncoderBound - BoundSafetyThreshold - currentPosition);
}
else if (DesiredPosition < currentPosition)
{
action = MotorAction.Reverse;
delta = Math.Min(delta, currentPosition - LowerEncoderBound - BoundSafetyThreshold);
}
// See if the move would take us out of bounds, and if so, don't do it!
if (action == MotorAction.Forward && currentPosition >= (UpperEncoderBound - BoundSafetyThreshold))
{
action = MotorAction.Stop;
}
else if (action == MotorAction.Reverse && currentPosition <= (LowerEncoderBound + BoundSafetyThreshold))
{
action = MotorAction.Stop;
}
double newRpm = GetSpeedSetting(delta);
// Perform the requested action
switch (action)
{
// Shut it down and consider this move a success
case MotorAction.Stop:
Driver.SoftHiZ();
MoveTask = null;
result = new FocusMoveResult(true, null);
MoveFinished?.Invoke(this, result);
return;
// Go forward if it isn't already going forward
case MotorAction.Forward:
if (Driver.Direction == MotorDirection.Reverse ||
status.MotorState == MotorActivity.Stopped ||
newRpm != CurrentSpeed)
{
Driver.Direction = MotorDirection.Forward;
Driver.DesiredSpeed = newRpm;
Driver.Run();
CurrentSpeed = newRpm;
}
break;
// Go backward if it isn't already going backward
case MotorAction.Reverse:
if (Driver.Direction == MotorDirection.Forward ||
status.MotorState == MotorActivity.Stopped ||
newRpm != CurrentSpeed)
{
Driver.Direction = MotorDirection.Reverse;
Driver.DesiredSpeed = newRpm;
Driver.Run();
CurrentSpeed = newRpm;
}
break;
}
Thread.Sleep(MoveLoopDelay);
}
// If we get here, the system requested a stop.
Driver.SoftHiZ();
MoveTask = null;
result = new FocusMoveResult(true, null);
MoveFinished?.Invoke(this, result);
return;
}