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; } }