public void Run(MotorAction command)
{
if (MotorController == null)
{
return;
}
if (command == MotorAction.FORWARD)
{
MotorController.SetPin(MotorIn2Pin, 0);
MotorController.SetPin(MotorIn1Pin, 1);
}
else if (command == MotorAction.BACKWARD)
{
MotorController.SetPin(MotorIn1Pin, 0);
MotorController.SetPin(MotorIn2Pin, 1);
}
else if (command == MotorAction.RELEASE)
{
MotorController.SetPin(MotorIn1Pin, 0);
MotorController.SetPin(MotorIn2Pin, 0);
}
}
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;
}
/// <summary>
/// 设置电机动作/适配奇果派的直流电机驱动，步进电机用不到，暂不支持
/// </summary>
/// <param name="channel">1-4</param>
/// <param name="action">前进后退，刹车就是不通电，不能刹车，英文注释看不懂了不知道咋刹</param>
/// <param name="speed">我也不知道这速度多快，100随便写的</param>
public void SetMotor(int channel, MotorAction action, int speed = 100)
{
if (channel < 1 || channel > 4)
{
return;
}
int pwm = 0;
int in1 = 0;
int in2 = 0;
switch (channel)
{
case 1:
pwm = 8; in2 = 9; in1 = 10;
break;
case 2:
pwm = 13; in2 = 12; in1 = 11;
break;
case 3:
pwm = 2; in2 = 3; in1 = 4;
break;
case 4:
pwm = 7; in2 = 6; in1 = 5;
break;
}
if (speed != _speed)
{
SetAngle(pwm, Math.Abs(speed > 256 ? 4096 : speed * 16));
_speed = Math.Abs(speed);
}
switch (action)
{
case MotorAction.Forward:
SetOff(in2);
SetOn(in1);
break;
case MotorAction.Break:
SetOff(in2);
SetOff(in1);
break;
case MotorAction.Backward:
SetOff(in1);
SetOn(in2);
break;
}
//在这添加限位器/初始化一个限位器并绑定事件吧
//初始化一个限位器并订阅限位器的事件
switch (action)
{
case MotorAction.Forward:
break;
case MotorAction.Backward:
break;
}
}
