public void SetState(MotorState state)
{
if (state == State)
{
return;
}
switch (state)
{
case MotorState.Forward:
System.Diagnostics.Debug.WriteLine("Moving the motor forwards");
backwardPin.Write(GpioPinValue.Low);
forwardPin.Write(GpioPinValue.High);
State = MotorState.Forward;
break;
case MotorState.Backward:
System.Diagnostics.Debug.WriteLine("Moving the motor backwards");
forwardPin.Write(GpioPinValue.Low);
backwardPin.Write(GpioPinValue.High);
State = MotorState.Backward;
break;
case MotorState.Stopped:
default:
System.Diagnostics.Debug.WriteLine("Stopping the motor");
forwardPin.Write(GpioPinValue.Low);
backwardPin.Write(GpioPinValue.Low);
State = MotorState.Stopped;
break;
}
}
public void MoveForward(float speed = 1f)
{
// Vector3 currentMoveVectorComparison = Vector3.Project(_moveVector, transform.TransformDirection(Vector3.forward));
// float comparisonVal = ForwardSpeed * speed;
_moveVector += transform.TransformDirection (Vector3.forward) * ForwardSpeed * speed;
_motorState = MotorState.MOVING;
}
public MotorState ConvertData(byte[] data)
{
if (data == null)
{
return(null);
}
if (data.Length >= 33)
{
MotorState obj = new MotorState();
obj.date = DateTime.Now.ToString("dd.MM.yyyy HH:mm:ss.") + DateTime.Now.Millisecond;
obj.address = data[0];
obj.command = data[1];
obj.state = data[2];
obj.line = loadInt(data, 3);
obj.x = loadInt(data, 7);
obj.y = loadInt(data, 11);
obj.z = loadInt(data, 15);
obj.xLimit = loadInt(data, 19);
obj.yLimit = loadInt(data, 23);
obj.zLimit = loadInt(data, 27);
obj.stateA = data[31];
obj.stateB = data[32];
return(obj);
}
return(null);
}
public void mainSheet(MotorState motorState)
{
if (motorStates[ArduinoCommunicationHandler.MAIN_SAIL_SHEET] != motorState)
{
motorStates[ArduinoCommunicationHandler.MAIN_SAIL_SHEET] = motorState;
setMotor(ArduinoCommunicationHandler.MAIN_SAIL_SHEET, motorState);
motorStates[ArduinoCommunicationHandler.HEAD_SAIL_SHEET] = motorState;
setMotor(ArduinoCommunicationHandler.HEAD_SAIL_SHEET, motorState);
// The top sail sheet works opposite of the main and head sail sheets.
MotorState topSailSheetMotorState;
switch (motorState)
{
case MotorState.Left:
topSailSheetMotorState = MotorState.Right;
break;
case MotorState.Right:
topSailSheetMotorState = MotorState.Left;
break;
case MotorState.Stop:
topSailSheetMotorState = MotorState.Stop;
break;
default:
throw new Exception("Motor state is not handled.");
}
motorStates[ArduinoCommunicationHandler.TOP_SAIL_SHEET] = topSailSheetMotorState;
setMotor(ArduinoCommunicationHandler.TOP_SAIL_SHEET, topSailSheetMotorState);
}
}
private void CorrectHorizontalAim(double x)
{
MotorState state = new MotorState();
if (Math.Abs(x) < 0.35)
{
state.Mode = MotorMode.On | MotorMode.Brake;
state.RunState = MotorRunState.Running;
state.TachoLimit = 0;
state.Power = 0;
state.TurnRatio = 80;
}
else
{
state.Mode = MotorMode.On | MotorMode.Brake;
state.RunState = MotorRunState.Running;
state.TachoLimit = 250;
state.Power = Math.Sign(x) * 30;
state.TurnRatio = 80;
}
if (nxtBrick.IsConnected)
{
nxtBrick.SetMotorState(Motor.C, state);
}
}
/// <summary>
/// Step the motor the specified number of steps.
/// </summary>
/// <param name="steps">
/// The number of steps to step the motor forward or backward. Set 0
/// to stop the motor.
/// </param>
/// <exception cref="ObjectDisposedException">
/// This instance has been disposed.
/// </exception>
public override void Step(Int32 steps)
{
if (base.IsDisposed)
{
throw new ObjectDisposedException("CyrusBuilt.MonoPi.Components.Motors.StepperMotorComponent");
}
if (steps == 0)
{
this.State = MotorState.Stop;
return;
}
// Perform step in positive or negative direction from current position.
base.OnMotorRotationStarted(new MotorRotateEventArgs(steps));
if (steps > 0)
{
for (Int32 i = 1; i <= steps; i++)
{
this.DoStep(true);
}
}
else
{
for (Int32 i = steps; i < 0; i++)
{
this.DoStep(false);
}
}
// Stop movement.
base.Stop();
base.OnMotorRotationStopped(EventArgs.Empty);
}
private void CorrectVerticalAim(double y)
{
MotorState state = new MotorState();
if (Math.Abs(y) < 0.35)
{
state.Mode = MotorMode.On | MotorMode.Brake;
state.RunState = MotorRunState.Idle;
state.TachoLimit = 0;
state.Power = 0;
state.TurnRatio = 80;
}
else
{
state.Mode = MotorMode.On | MotorMode.Brake;
state.RunState = MotorRunState.Running;
state.TachoLimit = 250;
state.Power = Math.Sign(y) * 20;
state.TurnRatio = 80;
}
if (nxtBrick.IsConnected)
{
nxtBrick.SetMotorState(Motor.B, state);
}
}
/// <summary>
/// Vehicle is moving forward if one of the motors is spinnign clockwise.
/// </summary>
/// <returns></returns>
public bool IsGoingForward()
{
MotorState motor1State = motor1.GetMotorState();
MotorState motor2State = motor2.GetMotorState();
return(motor1State == MotorState.SpinningClockWise || motor2State == MotorState.SpinningClockWise);
}
/// <summary>
/// Zobrazí stav konkrétního motoru
/// </summary>
/// <param name="motorView">vizualizace motoru</param>
/// <param name="state">stav</param>
/// <param name="message">zpráva ke stavu</param>
/// <param name="motorId">id motoru</param>
private void showMotorState(Label motorView, MotorState state, String message, MotorId motorId, int speed, int position)
{
switch (state)
{
case MotorState.error:
motorView.BackColor = Color.Red;
break;
case MotorState.enabled:
motorView.BackColor = Color.Green;
break;
case MotorState.disabled:
motorView.BackColor = Color.LightSlateGray;
break;
case MotorState.running:
motorView.BackColor = Color.Orange;
break;
}
this.Invoke((MethodInvoker) delegate
{
((Label)motorView.GetChildAtPoint(new Point(0, 0))).Text = speed.ToString();
motorView.Text = position.ToString();
toolTip.SetToolTip(motorView, "ID motoru: " + motorId + "\nStav motoru: " + state + "\nZpráva: " + message);
motorView.Update();
});
}
private void AttempToTransiteState(MotorState currentState, MotorState attemptState)
{
switch (currentState)
{
case MotorState.Idle:
switch (attemptState)
{
case MotorState.Forward:
m_currentState = MotorState.Forward;
PlayForwardAnimation();
break;
case MotorState.Backward:
m_currentState = MotorState.Backward;
PlayBackwardAnimation();
break;
case MotorState.Idle:
break;
}
break;
case MotorState.Forward:
switch (attemptState)
{
case MotorState.Forward:
break;
case MotorState.Backward:
m_currentState = MotorState.Idle;
ResetAllAnimation();
break;
case MotorState.Idle:
m_currentState = MotorState.Idle;
ResetAllAnimation();
break;
}
break;
case MotorState.Backward:
switch (attemptState)
{
case MotorState.Forward:
m_currentState = MotorState.Idle;
ResetAllAnimation();
break;
case MotorState.Backward:
break;
case MotorState.Idle:
m_currentState = MotorState.Idle;
ResetAllAnimation();
break;
}
break;
}
}
public void jibHoist(MotorState motorState)
{
if (motorStates[ArduinoCommunicationHandler.JIB_HOIST_ID] != motorState)
{
motorStates[ArduinoCommunicationHandler.JIB_HOIST_ID] = motorState;
setMotor(ArduinoCommunicationHandler.JIB_HOIST_ID, motorState);
}
}
public void jibTrim(MotorState motorState)
{
if (motorStates[ArduinoCommunicationHandler.HEAD_SAIL_SHEET] != motorState)
{
motorStates[ArduinoCommunicationHandler.HEAD_SAIL_SHEET] = motorState;
setMotor(ArduinoCommunicationHandler.HEAD_SAIL_SHEET, motorState);
}
}
/// <inheritdoc/>
public void Stop()
{
Microsoft.IoT.DeviceHelpers.TaskExtensions.UISafeWait(cancelRun);
for (int i = 0; i < 4; i++)
{
_coilpins[i].SetActiveDutyCyclePercentage(0.0);
}
_stepAsyncState = MotorState.Stop;
}
public MotorControllerComponent()
: base()
{
leftMotorState = MotorState.Disabled;
leftMotorSpeed = 0;
rightMotorState = MotorState.Disabled;
rightMotorSpeed = 0;
}
void Awake()
{
m_characterController = GetComponent <CharacterController>();
m_initialSlopeLimit = m_characterController.slopeLimit;
m_isDoubleJumpAvailabile = true;
m_motorState = new MotorState();
m_motorState.position = transform.localPosition;
}
public void Initialize(IEnumerable <MotorPin> pinMapping)
{
if (pinMapping is null)
{
throw new ArgumentNullException("PinMapping can not be null.");
}
this.pinMapping = pinMapping.ToDictionary(x => x.PinName);
this.motorState = GetMotorState();
}
public Robot(IDRobot id, string name)
{
IDRobot = id;
Name = name;
SpeedConfig = new SpeedConfig(500, 1000, 1000, 500, 1000, 1000);
AsserStats = new AsserStats();
IsSpeedAdvAdaptable = true;
foreach (MotorID moteur in Enum.GetValues(typeof(MotorID)))
{
MotorState.Add(moteur, false);
}
foreach (SensorOnOffID o in Enum.GetValues(typeof(SensorOnOffID)))
{
SensorsOnOffValue.Add(o, false);
}
foreach (ActuatorOnOffID o in Enum.GetValues(typeof(ActuatorOnOffID)))
{
ActuatorOnOffState.Add(o, false);
}
foreach (SensorColorID o in Enum.GetValues(typeof(SensorColorID)))
{
SensorsColorValue.Add(o, Color.Black);
}
AnalogicPinsValue = new Dictionary <Board, List <double> >();
AnalogicPinsValue.Add(Board.RecIO, new List <double>());
AnalogicPinsValue.Add(Board.RecMove, new List <double>());
for (int i = 0; i < 9; i++)
{
AnalogicPinsValue[Board.RecIO].Add(0);
AnalogicPinsValue[Board.RecMove].Add(0);
}
NumericPinsValue = new Dictionary <Board, List <Byte> >();
NumericPinsValue.Add(Board.RecIO, new List <byte>());
NumericPinsValue.Add(Board.RecMove, new List <byte>());
for (int i = 0; i < 3 * 2; i++)
{
NumericPinsValue[Board.RecIO].Add(0);
NumericPinsValue[Board.RecMove].Add(0);
}
BatterieVoltage = 0;
Graph = null;
TrajectoryFailed = false;
TrajectoryCutOff = false;
TrajectoryRunning = null;
}
/// <summary>
/// Stops the motor's movement.
/// </summary>
public void Stop()
{
if (this.State == MotorState.Stop)
{
return;
}
MotorState oldState = this.State;
this.State = MotorState.Stop;
this.OnMotorStateChanged(new MotorStateChangeEventArgs(oldState, this.State));
}
/// <summary>
/// Tells the motor to reverse direction.
/// </summary>
public void Reverse()
{
if (this.State == MotorState.Reverse)
{
return;
}
MotorState oldState = this.State;
this.State = MotorState.Reverse;
this.OnMotorStateChanged(new MotorStateChangeEventArgs(oldState, this.State));
}
/// <summary>
/// Tells the motor to move forward.
/// </summary>
public void Forward()
{
if (this.State == MotorState.Forward)
{
return;
}
MotorState oldState = this.State;
this.State = MotorState.Forward;
this.OnMotorStateChanged(new MotorStateChangeEventArgs(oldState, this.State));
}
protected void MotorInput()
{
if (Input.GetKey(ForwardAxisKey))
{
state = MotorState.DriveForward;
}
else if (Input.GetKey(BackAxisKey))
{
state = MotorState.DriveBack;
}
else
{
state = MotorState.DriveOff;
}
}
private void StopAim()
{
MotorState state = new MotorState();
state.Mode = MotorMode.On | MotorMode.Brake;
state.RunState = MotorRunState.Running;
state.TachoLimit = 0;
state.Power = 0;
state.TurnRatio = 80;
if (nxtBrick.IsConnected)
{
nxtBrick.SetMotorState(Motor.All, state, true);
}
}
/// <summary>
/// Get motor state.
/// </summary>
///
/// <param name="motor">Motor to get state for.</param>
/// <param name="state">Motor's state.</param>
///
/// <returns>Returns <b>true</b> if command was executed successfully or <b>false</b> otherwise.</returns>
///
public bool GetMotorState(Motor motor, out MotorState state)
{
state = new MotorState( );
// check motor port
if (motor == Motor.All)
{
//throw new ArgumentException( "Motor state can be retrieved for one motor only" );
Debug.WriteLine("Motor state can be retrieved for one motor only");
}
byte[] command = new byte[3];
byte[] reply = new byte[25];
// prepare message
command[0] = (byte)NXTCommandType.DirectCommand;
command[1] = (byte)NXTDirectCommand.GetOutputState;
command[2] = (byte)motor;
if (SendCommand(command, reply))
{
state.Power = (sbyte)reply[4];
state.Mode = (MotorMode)reply[5];
state.Regulation = (MotorRegulationMode)reply[6];
state.TurnRatio = (sbyte)reply[7];
state.RunState = (MotorRunState)reply[8];
// tacho limit
state.TachoLimit = reply[9] | (reply[10] << 8) |
(reply[11] << 16) | (reply[12] << 24);
// tacho count
state.TachoCount = reply[13] | (reply[14] << 8) |
(reply[15] << 16) | (reply[16] << 24);
// block tacho count
state.BlockTachoCount = reply[17] | (reply[18] << 8) |
(reply[19] << 16) | (reply[20] << 24);
// rotation count
state.RotationCount = reply[21] | (reply[22] << 8) |
(reply[23] << 16) | (reply[24] << 24);
return(true);
}
return(false);
}
private static double ConvertRegistersToMotorSpeed(byte speedRegister, MotorState state)
{
switch (state)
{
case MotorState.Brake:
case MotorState.Disabled:
return(0);
case MotorState.Forwards:
return(speedRegister / 255.0);
case MotorState.Reverse:
return(-speedRegister / 255.0);
default:
throw new InvalidOperationException();
}
}
public PlatformMotor(GridProgramRef gridProgramRef, string pistonGroupName, MotorConfig motorConfig)
{
// Safe guard: Throw an exception if GridTerminalSystem is null
if (gridProgramRef.GridTerminalSystem == null)
{
throw new ArgumentNullException("Passed GTS reference was null.");
}
if (gridProgramRef.Echo == null)
{
throw new ArgumentNullException("Passed Echo reference was null.");
}
// Initialize initial variables
PistonGroupName = pistonGroupName;
PistonList = new List <IMyPistonBase>();
MotorState = MotorState.Idle;
GridTerminalSystem = gridProgramRef.GridTerminalSystem;
Echo = gridProgramRef.Echo;
MotorConfig = motorConfig;
// Validate motor config
if (MotorConfig.MotorSpeed < 0)
{
throw new ArgumentException("MotorConfig: Velocity must be a signed number.");
}
if (MotorConfig.MotorMaxElevation < 0)
{
throw new ArgumentException("MotorConfig: Max elevation must be a signed number.");
}
if (MotorConfig.MotorMinElevation < 0)
{
throw new ArgumentException("MotorConfig: Min elevation must be a signed number.");
}
// Validate motor config end
// Safe guard: Throw an exception if group is non existent
var blocks = GridTerminalSystem.GetBlockGroupWithName(PistonGroupName);
if (blocks == null)
{
throw new ArgumentException("Piston block group was not found.");
}
blocks.GetBlocksOfType(PistonList);
ApplyConfig();
}
private void HandleWallRun()
{
RaycastHit wallHit;
if (CheckCollision(transform.right, out wallHit))
{
// Pass the hit point's normal to the wallrun motor
wallRunMotor.wallHit = wallHit;
wallRunMotor.wallRunForward = Vector3.Cross(Vector3.up, wallHit.normal);
currentState = MotorState.wallRunning;
}
else if (CheckCollision(-transform.right, out wallHit))
{
// Pass the hit point's normal to the wallrun motor
wallRunMotor.wallHit = wallHit;
wallRunMotor.wallRunForward = Vector3.Cross(wallHit.normal, Vector3.up);
currentState = MotorState.wallRunning;
}
}
/// <summary>
/// Step size (microns) for the focuser. Raises an exception if
/// the focuser does not intrinsically know what the step size is.
/// </summary>
public void Move(int value)
{
CheckConnected("Move");
// Next two lines removed to implement IFocuserV3 requirement
// if (tempComp)
// throw new InvalidOperationException("Move not allowed when temperature compensation is active");
if (Absolute)
{
TL.LogMessage("Move Absolute", value.ToString());
Target = Truncate(0, value, MaxStep);
RateOfChange = 40;
}
else
{
TL.LogMessage("Move Relative", value.ToString());
Target = 0;
_position = Truncate(-MaxStep, value, MaxStep);
RateOfChange = 40;
}
motorState = MotorState.moving;
}
/// <summary>
/// Set motor state.
/// </summary>
///
/// <param name="motor">Motor to set state for.</param>
/// <param name="state">Motor's state to set.</param>
///
/// <returns>Returns <b>true</b> if command was executed successfully or <b>false</b> otherwise.</returns>
///
public bool SetMotorState(Motor motor, MotorState state)
{
byte[] command = new byte[12];
// prepare message
command[0] = (byte)NXTCommandType.DirectCommand;
command[1] = (byte)NXTDirectCommand.SetOutputState;
command[2] = (byte)motor;
command[3] = (byte)state.Power;
command[4] = (byte)state.Mode;
command[5] = (byte)state.Regulation;
command[6] = (byte)state.TurnRatio;
command[7] = (byte)state.RunState;
// tacho limit
command[8] = (byte)(state.TachoLimit & 0xFF);
command[9] = (byte)((state.TachoLimit >> 8) & 0xFF);
command[10] = (byte)((state.TachoLimit >> 16) & 0xFF);
command[11] = (byte)((state.TachoLimit >> 24) & 0xFF);
return(SendCommand(command, new byte[3]));
}
public void ChangeMotorState(MotorState state)
{
switch (state)
{
case MotorState.Idle:
gameObject.GetComponent <SkeletonAnimation>().AnimationName = null;
break;
case MotorState.Run:
gameObject.GetComponent <SkeletonAnimation>().AnimationName = "run";
break;
case MotorState.Jump:
gameObject.GetComponent <SkeletonAnimation>().AnimationName = "jump";
break;
case MotorState.Fall:
gameObject.GetComponent <SkeletonAnimation>().AnimationName = "fall";
break;
}
}
private void CheckGrapple()
{
RaycastHit hit;
bool inRange = Physics.SphereCast(transform.position, grappleSphereCastRadius, Camera.main.transform.forward, out hit, grappleMotor.maxRopeLength, grappleMotor.grappleLayer);
UIAnimator.SetBool("InRange", inRange);
// Check if the player wants to grapple
if (Input.GetMouseButtonDown(0) && inRange)
{
grappleMotor.InitializeGrapple(hit.transform.gameObject);
audioSrc.PlayOneShot(grappleMotor.grappleSFX);
currentState = MotorState.grappling;
}
// Let go of left click, no more grappling
else if (Input.GetMouseButtonUp(0))
{
grappleMotor.DisableGrappleUI();
currentState = MotorState.falling;
}
}
void Update()
{
var data = GameManager.GetInstance(player_number).GetMovementManager().GetState(laneId);
var state = data.Item1;
var timestamp = data.Item2;
currentPosition = transform.position.z;
if (lastTimestamp != timestamp)
{
currentState = MotorState.Moving;
if (this.player_number == 0)
{
targetPosition = initialPosition.z + state.position;
}
else
{
targetPosition = initialPosition.z - state.position;
}
targetPosition = calculateLimit(currentPosition, targetPosition);
}
if (Mathf.Approximately(currentPosition, targetPosition))
{
currentState = MotorState.Idle;
}
deltaPosition = targetPosition - currentPosition;
if (currentState == MotorState.Moving)
{
transform.Translate(0, deltaPosition * Time.deltaTime * speed, 0);
}
if (state.kick)
{
kickSimulator.Kick(timestamp);
}
}
void LoadMotorstateOnRestart()
{
if (actualMotorState == null)
{
try
{
Stream stream = new FileStream("Motorstate.bin", FileMode.Open);
BinaryFormatter formatter = new BinaryFormatter();
actualMotorState = (MotorState)formatter.Deserialize(stream);
stream.Close();
}
catch (Exception e)
{
Console.WriteLine("Kein Motorstate ladbar, das ist beim ersten Mal ok.");
Console.WriteLine(e.Message);
actualMotorState = new MotorState()
{
Motorstellung = 0, Position = 0, Offset = 0
};
}
}
}
public override void GuiParameters()
{
base.GuiParameters();
state = EditorGUILayout.ObjectField(state, typeof(MotorState), false) as MotorState;
}
private void ChangeState(MotorState newState)
{
// no change...
if (motorState == newState)
{
return;
}
if (IsSlipping())
{
if (onSlippingEnd != null)
{
onSlippingEnd();
}
}
if (IsJumping())
{
if (onJumpEnd != null)
{
onJumpEnd();
}
}
if (IsDashing())
{
if (onDashEnd != null)
{
onDashEnd();
}
}
// set
motorState = newState;
if (IsSlipping())
{
if (onSlipping != null)
{
onSlipping();
}
}
if (IsJumping())
{
if (onJump != null)
{
onJump();
}
}
if (IsDashing())
{
if (onDash != null)
{
onDash();
}
}
}
protected override void OnPollDataReceived(int pollID, byte i2cAddress, byte register, int[] values, object data)
{
if (values == null) return;
int value = values[0];
switch (register)
{
case I2CRegisters.LeftMotorState:
if (leftMotorState == (MotorState)value) return;
if (!Enum.IsDefined(typeof(MotorState), value))
throw new RobotException("Invalid MotorState");
leftMotorState = (MotorState)value;
OnNotifyPropertyChanged("LeftMotorState");
break;
case I2CRegisters.LeftMotorSpeed:
double x = ConvertRegistersToMotorSpeed((byte)value, leftMotorState);
if (leftMotorSpeed == x) return;
leftMotorSpeed = x;
OnNotifyPropertyChanged("LeftMotorState");
break;
case I2CRegisters.RightMotorState:
if (rightMotorState == (MotorState)value) return;
if (!Enum.IsDefined(typeof(MotorState), value))
throw new RobotException("Invalid MotorState");
rightMotorState = (MotorState)value;
OnNotifyPropertyChanged("RightMotorState");
break;
case I2CRegisters.RightMotorSpeed:
x = ConvertRegistersToMotorSpeed((byte)value, rightMotorState);
if (rightMotorSpeed == x) return;
rightMotorSpeed = x;
OnNotifyPropertyChanged("RightMotorSpeed");
break;
}
}
/// <summary>
/// Ticks 10 times a second, updating the focuser position and IsMoving properties
/// </summary>
private void MoveTimer_Tick(object source, System.Timers.ElapsedEventArgs e)
{
// Change at introduction of IFocuserV3 - only allow random temperature induced changes when the motor is in the idle state
// This is because IFocuser V3 allows moves when temperature compensation is active
if (motorState == MotorState.idle)
{
//Create random temperature change
if (DateTime.Now.Subtract(lastTempUpdate).TotalSeconds > TempPeriod)
{
lastTempUpdate = DateTime.Now;
// apply a random change to the temperature
double tempOffset = (RandomGenerator.NextDouble() - 0.5);// / 10.0;
Temperature = Math.Round(Temperature + tempOffset, 2);
// move the focuser target to track the temperature if required
if (tempComp)
{
var dt = (int)((Temperature - _lastTemp) * TempSteps);
if (dt != 0)// return;
{
Target += dt;
_lastTemp = Temperature;
}
}
}
}
if (Target > MaxStep)
{
Target = MaxStep; // Condition target within the acceptable range
}
if (Target < 0)
{
Target = 0;
}
if (_position != Target) //Actually move the focuse if necessary
{
TL.LogMessage("Moving", "LastOffset, Position, Target RateOfChange " + LastOffset + " " + _position + " " + Target + " " + RateOfChange);
if (Math.Abs(_position - Target) <= RateOfChange)
{
_position = Target;
TL.LogMessage("Moving", " Set position = target");
}
else
{
_position += (_position > Target) ? -RateOfChange : RateOfChange;
TL.LogMessage("Moving", " Updated position = " + _position);
}
TL.LogMessage("Moving", " New position = " + _position);
}
if (KeepMoving & (DateTime.Now.Subtract(MouseDownTime).TotalSeconds > 0.5))
{
Target = (Math.Sign(LastOffset) > 0) ? MaxStep : 0;
MouseDownTime = DateTime.Now;
if (RateOfChange < 100)
{
RateOfChange = (int)Math.Ceiling((double)RateOfChange * 1.2);
}
TL.LogMessage("KeepMoving", "LastOffset, Position, Target, RateOfChange MouseDownTime " + LastOffset + " " + _position + " " + Target + " " + RateOfChange + " " + MouseDownTime.ToLongTimeString());
}
// handle MotorState
switch (motorState)
{
case MotorState.moving:
if (_position == Target)
{
motorState = MotorState.settling;
settleFinishTime = DateTime.Now + TimeSpan.FromMilliseconds(settleTime);
TL.LogMessage("MoveTimer", "Settle start, time " + settleTime.ToString());
}
return;
case MotorState.settling:
if (settleFinishTime < DateTime.Now)
{
motorState = MotorState.idle;
TL.LogMessage("MoveTimer", "settle finished");
}
return;
}
}
public void topSailTrim(MotorState motorState)
{
if(motorStates[ArduinoCommunicationHandler.TOP_SAIL_SHEET] != motorState)
{
motorStates[ArduinoCommunicationHandler.TOP_SAIL_SHEET] = motorState;
setMotor(ArduinoCommunicationHandler.TOP_SAIL_SHEET, motorState);
}
}
/// <summary>
/// Get motor state.
/// </summary>
///
/// <param name="motor">Motor to get state for.</param>
/// <param name="state">Motor's state.</param>
///
/// <returns>Returns <b>true</b> if command was executed successfully or <b>false</b> otherwise.</returns>
///
public bool GetMotorState( Motor motor, out MotorState state )
{
state = new MotorState( );
// check motor port
if ( motor == Motor.All )
{
throw new ArgumentException( "Motor state can be retrieved for one motor only" );
}
byte[] command = new byte[3];
byte[] reply = new byte[25];
// prepare message
command[0] = (byte) NXTCommandType.DirectCommand;
command[1] = (byte) NXTDirectCommand.GetOutputState;
command[2] = (byte) motor;
if ( SendCommand( command, reply ) )
{
state.Power = (sbyte) reply[4];
state.Mode = (MotorMode) reply[5];
state.Regulation = (MotorRegulationMode) reply[6];
state.TurnRatio = (sbyte) reply[7];
state.RunState = (MotorRunState) reply[8];
// tacho limit
state.TachoLimit = reply[9] | ( reply[10] << 8 ) |
( reply[11] << 16 ) | ( reply[12] << 24 );
// tacho count
state.TachoCount = reply[13] | ( reply[14] << 8 ) |
( reply[15] << 16 ) | ( reply[16] << 24 );
// block tacho count
state.BlockTachoCount = reply[17] | ( reply[18] << 8 ) |
( reply[19] << 16 ) | ( reply[20] << 24 );
// rotation count
state.RotationCount = reply[21] | ( reply[22] << 8 ) |
( reply[23] << 16 ) | ( reply[24] << 24 );
return true;
}
return false;
}
private static void ConvertMotorSpeedToRegisters(double speed, out byte speedRegister, out MotorState state)
{
if (speed < -1)
{
speed = -1;
}
if (speed > +1)
{
speed = +1;
}
state = MotorState.Disabled;
if (speed < 0)
{
state = MotorState.Reverse;
}
else if (speed > 0)
{
state = MotorState.Forwards;
}
speedRegister = (byte)Math.Round(Math.Abs(speed) * 255);
}
/// <summary>
/// Zobrazí stav konkrétního motoru
/// </summary>
/// <param name="motorView">vizualizace motoru</param>
/// <param name="state">stav</param>
/// <param name="message">zpráva ke stavu</param>
/// <param name="motorId">id motoru</param>
private void showMotorState(Label motorView, MotorState state, String message, MotorId motorId, int speed, int position) {
switch (state)
{
case MotorState.error:
motorView.BackColor = Color.Red;
break;
case MotorState.enabled:
motorView.BackColor = Color.Green;
break;
case MotorState.disabled:
motorView.BackColor = Color.LightSlateGray;
break;
case MotorState.running:
motorView.BackColor = Color.Orange;
break;
}
this.Invoke((MethodInvoker)delegate
{
((Label)motorView.GetChildAtPoint(new Point(0,0))).Text = speed.ToString();
motorView.Text = position.ToString();
toolTip.SetToolTip(motorView, "ID motoru: " + motorId + "\nStav motoru: " + state + "\nZpráva: " + message);
motorView.Update();
});
}
private char getMotorRotationFlag(MotorState motorState)
{
switch (motorState)
{
case MotorState.Left:
return ArduinoCommunicationHandler.ROTATE_LEFT;
case MotorState.Right:
return ArduinoCommunicationHandler.ROTATE_RIGHT;
case MotorState.Stop:
// this value is not used anyway so we will return rotate left as a filler
return ArduinoCommunicationHandler.ROTATE_LEFT;
default:
throw new Exception("Motor state is not handled.");
}
}
public void MoveBack(float speed = 1f)
{
_moveVector += transform.TransformDirection (Vector3.back) * BackSpeed * speed;
_motorState = MotorState.MOVING;
}
/// <summary>
/// Set motor state.
/// </summary>
///
/// <param name="motor">Motor to set state for.</param>
/// <param name="state">Motor's state to set.</param>
///
/// <returns>Returns <b>true</b> if command was executed successfully or <b>false</b> otherwise.</returns>
///
public bool SetMotorState( Motor motor, MotorState state )
{
return SetMotorState( motor, state, true );
}
/// <summary>
/// Zobrazí stav konkrétního motoru
/// </summary>
/// <param name="state">stav</param>
/// <param name="message">zpráva ke stavu</param>
/// <param name="motorId">id motoru</param>
/// <param name="speed">aktuální rychlost motoru</param>
/// <param name="position">aktuální pozice motoru</param>
/// <param name="speedRelative">relativní rychlost (-100 až 100)</param>
/// <param name="angle">poloha motoru jako úhel</param>
public void motorStateChanged(MotorState state, string message, MotorId motorId, int speed, int position, int speedRelative, int angle)
{
showMotorState(motorViews[motorId], state, message, motorId, speed, position);
changeVisualization(motorId, speedRelative, angle);
}
/// <summary>
/// Set motor state.
/// </summary>
///
/// <param name="motor">Motor to set state for.</param>
/// <param name="state">Motor's state to set.</param>
/// <param name="waitReply">Wait reply from NXT (safer option) or not (faster option).</param>
///
/// <returns>Returns <b>true</b> if command was executed successfully or <b>false</b> otherwise.</returns>
///
public bool SetMotorState( Motor motor, MotorState state, bool waitReply )
{
byte[] command = new byte[12];
// prepare message
command[0] = (byte) ( ( waitReply ) ? NXTCommandType.DirectCommand : NXTCommandType.DirectCommandWithoutReply );
command[1] = (byte) NXTDirectCommand.SetOutputState;
command[2] = (byte) motor;
command[3] = (byte) state.Power;
command[4] = (byte) state.Mode;
command[5] = (byte) state.Regulation;
command[6] = (byte) state.TurnRatio;
command[7] = (byte) state.RunState;
// tacho limit
command[8] = (byte) ( state.TachoLimit & 0xFF );
command[9] = (byte) ( ( state.TachoLimit >> 8 ) & 0xFF );
command[10] = (byte) ( ( state.TachoLimit >> 16 ) & 0xFF );
command[11] = (byte) ( ( state.TachoLimit >> 24 ) & 0xFF );
return SendCommand( command, new byte[3] );
}
public MotorState ConvertData(byte[] data)
{
if (data == null) return null;
if (data.Length >= 33)
{
MotorState obj = new MotorState();
obj.date = DateTime.Now.ToString("dd.MM.yyyy HH:mm:ss.") + DateTime.Now.Millisecond;
obj.address = data[0];
obj.command = data[1];
obj.state = data[2];
obj.line = loadInt(data, 3);
obj.x = loadInt(data, 7);
obj.y = loadInt(data, 11);
obj.z = loadInt(data, 15);
obj.xLimit = loadInt(data, 19);
obj.yLimit = loadInt(data, 23);
obj.zLimit = loadInt(data, 27);
obj.stateA = data[31];
obj.stateB = data[32];
return obj;
}
return null;
}
/// <summary>
/// Initializes a new instance of the <see cref="CyrusBuilt.MonoPi.Components.Motors.MotorStateChangeEventArgs"/>
/// class with the old and new states.
/// </summary>
/// <param name="oldState">
/// The state the motor was in prior to the change.
/// </param>
/// <param name="newState">
/// The current state of the motor since the change occurred.
/// </param>
public MotorStateChangeEventArgs(MotorState oldState, MotorState newState)
: base() {
this._oldState = oldState;
this._newState = newState;
}
private static double ConvertRegistersToMotorSpeed(byte speedRegister, MotorState state)
{
switch (state)
{
case MotorState.Brake:
case MotorState.Disabled:
return 0;
case MotorState.Forwards:
return speedRegister / 255.0;
case MotorState.Reverse:
return -speedRegister / 255.0;
default:
throw new InvalidOperationException();
}
}
public void topHoist(MotorState motorState)
{
if (motorStates[ArduinoCommunicationHandler.TOP_SAIL_HOIST_ID] != motorState)
{
motorStates[ArduinoCommunicationHandler.TOP_SAIL_HOIST_ID] = motorState;
setMotor(ArduinoCommunicationHandler.TOP_SAIL_HOIST_ID, motorState);
}
}
private static void ConvertMotorSpeedToRegisters(double speed, out byte speedRegister, out MotorState state)
{
if (speed < -1) speed = -1;
if (speed > +1) speed = +1;
state = MotorState.Disabled;
if (speed < 0) state = MotorState.Reverse;
else if (speed > 0) state = MotorState.Forwards;
speedRegister = (byte)Math.Round(Math.Abs(speed) * 255);
}
private char getMotorOnOffFlag(MotorState motorState)
{
switch (motorState)
{
case MotorState.Left:
case MotorState.Right:
return ArduinoCommunicationHandler.MOTOR_ON;
case MotorState.Stop:
return ArduinoCommunicationHandler.MOTOR_OFF;
default:
throw new Exception("Motor state not handled.");
}
}
/// <summary>
/// Step the motor the specified number of steps.
/// </summary>
/// <param name="steps">
/// The number of steps to step the motor forward or backward. Set 0
/// to stop the motor.
/// </param>
/// <exception cref="ObjectDisposedException">
/// This instance has been disposed.
/// </exception>
public override void Step(Int32 steps)
{
if (base.IsDisposed) {
throw new ObjectDisposedException("CyrusBuilt.MonoPi.Components.Motors.StepperMotorComponent");
}
if (steps == 0) {
this.State = MotorState.Stop;
return;
}
// Perform step in positive or negative direction from current position.
base.OnMotorRotationStarted(new MotorRotateEventArgs(steps));
if (steps > 0) {
for (Int32 i = 1; i <= steps; i++) {
this.DoStep(true);
}
}
else {
for (Int32 i = steps; i < 0; i++) {
this.DoStep(false);
}
}
// Stop movement.
base.Stop();
base.OnMotorRotationStopped(EventArgs.Empty);
}
private void setMotor(char motorId, MotorState motorState)
{
communicationHandler.sendMessage(motorId,
getMotorOnOffFlag(motorState),
getMotorRotationFlag(motorState));
}
public void MoveRight(float speed = 1f)
{
_moveVector += transform.TransformDirection (Vector3.right) * SideSpeed * speed;
_motorState = MotorState.MOVING;
}
/// <summary>
/// <para>[Internal] Run motors on the NXT brick.</para>
/// </summary>
/// <param name="motorPort">MotorPort Port</param>
/// <param name="power">Power Set Point, between -100 and 100.</param>
/// <param name="mode">Mode</param>
/// <param name="regulationMode">Regulation Mode</param>
/// <param name="turnRatio">Turn Ratio, between -100 and 100.</param>
/// <param name="runState">Run State</param>
/// <param name="tachoLimit">Tacho Limit, 0=run forever</param>
/// <returns>Returns true if operation was a success, false otherwise. If false, check LastError.</returns>
internal void SetOutputState(Motor motorPort
, sbyte power, MotorMode mode
, MotorReg regulationMode, sbyte turnRatio
, MotorState runState, UInt32 tachoLimit)
{
if (power < -100) power = -100;
if (power > 100) power = 100;
if (turnRatio < -100) turnRatio = -100;
if (turnRatio > 100) turnRatio = 100;
byte[] request = new byte[12];
request[0] = (byte)(0x00);
request[1] = (byte)(DirectCommand.SetOutputState);
request[2] = (byte)motorPort;
request[3] = (byte)power;
request[4] = (byte)mode;
request[5] = (byte)regulationMode;
request[6] = (byte)turnRatio;
request[7] = (byte)runState;
SetUInt32(tachoLimit, request, 8);
CompleteRequest(request);
}
public void ResetMoveVector()
{
_moveVector = Vector3.zero;
_motorState = MotorState.STOPPED;
//Brake();
}
public void Initialize(Rigidbody ConnectedBody, Vector3 Anchor, Vector3 RemoteAnchor, Single Flexibility, Single ForceConstant, Single MaxMotorForce, Single MotorSpeed, Single LowerLimit, Single UpperLimit, Single DampingRate, Boolean CenterOnStop, Boolean DisableUpwardMovement, Single InitialLength = -1)
{
InitialTranform = transform.parent.localPosition + transform.localPosition;
this.ConnectedBody = ConnectedBody;
this.Anchor = Anchor;
this.RemoteAnchor = RemoteAnchor;
this.Flexibility = Flexibility;
this.ForceConstant = ForceConstant;
this.MaxMotorForce = MaxMotorForce;
this.MotorSpeed = MotorSpeed;
this.LowerLimit = LowerLimit;
this.UpperLimit = UpperLimit;
this.DampingRate = DampingRate;
this.CenterOnStop = CenterOnStop;
this.DisableUpwardMovement = DisableUpwardMovement;
if (InitialLength == -1)
{
this.InitialLength = (RemoteAnchor - transform.parent.localPosition - transform.localPosition - Anchor).magnitude;
this.FixedDefaultBaseLength = true;
}
else
{
this.InitialLength = InitialLength;
this.FixedDefaultBaseLength = false;
}
this.State = MotorState.Stopped;
this.DesiredLength = this.InitialLength;
}
public void ThrottleForward(float throttle = 0f)
{
_moveVector += transform.TransformDirection (Vector3.forward) * throttle * ((throttle > 0) ? ForwardSpeed : BackSpeed);
_motorState = MotorState.MOVING;
}
/// <summary>
/// Releaseses all resources used this object.
/// </summary>
/// <param name="disposing">
/// Set true if disposing managed resources in addition to unmanaged.
/// </param>
protected override void Dispose(Boolean disposing)
{
if (disposing) {
this._sequenceIndex = 0;
this.State = MotorState.Stop;
if ((this._controlThread != null) && (this._controlThread.IsAlive)) {
try {
this._controlThread.Abort();
}
catch (ThreadAbortException) {
}
finally {
this._controlThread = null;
}
}
if (this._pins != null) {
foreach (GpioMem pin in this._pins) {
pin.Dispose();
}
Array.Clear(this._pins, 0, this._pins.Length);
this._pins = null;
}
}
base.Dispose(disposing);
}
public void ThrottleSideways(float throttle = 0f)
{
_moveVector += transform.TransformDirection (Vector3.forward) * throttle * SideSpeed;
_motorState = MotorState.MOVING;
}
