public bool CloseAmpC()
{
connected = false;
enabled = false;
Status = MotorStatus.Disable;
return(connected);
}
/// <summary>
/// Read a motor status
/// </summary>
/// <param name="port">The Motor port to use, can be MotorLeft or MotorRight</param>
/// <returns>Returns MotorStatus containing the status of the motor</returns>
public MotorStatus GetMotorStatus(MotorPort port)
{
MotorStatus motorStatus = new MotorStatus();
SpiMessageType message_type = (port == MotorPort.MotorRight) ? SpiMessageType.GetMotorStatusRight : SpiMessageType.GetMotorStatusLeft;
byte[] outArray = { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
var reply = SpiTransferArray(outArray);
if (reply[3] == SpiCorrectDataReturned)
{
motorStatus.Speed = reply[5];
if ((motorStatus.Speed & 0x80) > 0)
{
motorStatus.Speed = -motorStatus.Speed;
}
motorStatus.Encoder = (int)(BinaryPrimitives.ReadInt32BigEndian(new Span <byte>(reply).Slice(6)) / MotorTicksPerDegree);
motorStatus.Dps = ((reply[10] << 8) | reply[11]);
if ((motorStatus.Dps & 0x8000) > 0)
{
motorStatus.Dps = motorStatus.Dps - 0x10000;
}
motorStatus.Flags = (MotorStatusFlags)reply[4];
}
else
{
throw new IOException($"{nameof(GetMotorStatus)} error: no SPI response");
}
return(motorStatus);
}
public void reset()
{
lock ( mutex_ )
{
propulsion_model_.x.Initialize();
propulsion_model_.x_pred.Initialize();
propulsion_model_.u.Initialize();
propulsion_model_.y.Initialize();
wrench_ = new Wrench();
motor_status_ = new MotorStatus();
motor_status_.voltage = new float[4];
motor_status_.frequency = new float[4];
motor_status_.current = new float[4];
supply_ = new Supply();
supply_.voltage = new float[1];
supply_.voltage[0] = (float)initial_voltage_;
last_command_time_ = ROS.GetTime();
command_queue_ = new Queue <MotorPWM> ();
}
}
/// <summary>
/// Change the polatity of the motor
/// </summary>
/// <param name="polarity">Polarity of the motor, backward, forward or opposite</param>
public void SetPolarity(Polarity polarity)
{
try
{
MotorStatus motorstatus = _goPiGo.GetMotorStatus(Port);
switch (polarity)
{
case Polarity.Backward:
if (motorstatus.Speed > 0)
{
_goPiGo.SetMotorPower(Port, -Speed);
}
break;
case Polarity.Forward:
if (motorstatus.Speed < 0)
{
_goPiGo.SetMotorPower(Port, -Speed);
}
break;
case Polarity.OppositeDirection:
_goPiGo.SetMotorPower(Port, -Speed);
break;
default:
break;
}
}
catch (IOException)
{
}
}
public void Move(Direction direction)
{
motorStatus = GetMotorStatus();
if (motorStatus == MotorStatus.MOVING)
{
return;
}
DoorStatus doorStatus = door.GetDoorStatus();
if (doorStatus == DoorStatus.OPEND)
{
door.Close();
}
Console.WriteLine("Closing the door");
SetMotorStatus(MotorStatus.MOVING);
//concret class implements this method
MoveMotor(direction);
SetMotorStatus(MotorStatus.STOPPED);
door.Open();
Console.WriteLine("Opening the door");
}
public bool InitAmpC()
{
connected = true;
InitAmpcStatus();
EnableAmpC();
Status = MotorStatus.Connect;
return(connected);
}
public MotorStateBase(Player2DMotor motor, Player2DFeedbacks feedback)
{
this.motor = motor;
this.feedback = feedback;
motorStatus = motor.status;
raycaster = motor.raycaster;
settings = GameSettings.instance;
}
/// <summary>
/// Read a motor status
/// </summary>
/// <param name="port">The motor port (one at a time). PortA, PortB, PortC, or PortD.</param>
/// <returns>MotorStatus containing the status of the motor</returns>
public MotorStatus GetMotorStatus(byte port)
{
MotorStatus motorStatus = new MotorStatus();
SpiMessageType message_type = SpiMessageType.None;
if (port == (byte)MotorPort.PortA)
{
message_type = SpiMessageType.GetMotorAStatus;
}
else if (port == (byte)MotorPort.PortB)
{
message_type = SpiMessageType.GetMotorBStatus;
}
else if (port == (byte)MotorPort.PortC)
{
message_type = SpiMessageType.GetMotorCStatus;
}
else if (port == (byte)MotorPort.PortD)
{
message_type = SpiMessageType.GetMotorDStatus;
}
else
{
throw new IOException(
$"{nameof(GetMotorStatus)} error. Must be one motor port at a time. PORT_A, PORT_B, PORT_C, or PORT_D.");
}
byte[] outArray = { SpiAddress, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
var reply = SpiTransferArray(outArray);
if (reply[3] == 0xA5)
{
motorStatus.Speed = reply[5];
if ((motorStatus.Speed & 0x80) > 0)
{
motorStatus.Speed = -motorStatus.Speed;
}
motorStatus.Encoder = (reply[6] << 24) | (reply[7] << 16) | (reply[8] << 8) | reply[9];
// negative should be managed
// if ((motorStatus.Encoder & 0x80000000) > 0)
// motorStatus.Encoder = motorStatus.Encoder - 0x100000000;
motorStatus.Dps = ((reply[10] << 8) | reply[11]);
if ((motorStatus.Dps & 0x8000) > 0)
{
motorStatus.Dps = motorStatus.Dps - 0x10000;
}
motorStatus.Flags = (MotorStatusFlags)reply[4];
}
else
{
throw new IOException($"{nameof(GetMotorStatus)} error: no SPI response");
}
return(motorStatus);
}
public bool ResetAmpC()
{
enabled = false;
InitAmpC();
Status = MotorStatus.Connect;
PIDVel.Reset();
PIDPos.Reset();
EnableAmpC();
return(enabled);
}
public static MotorInformation GetMotorInformation(Dictionary <AxisTypes, MotorParameter> motorParaMap, MotorInfoMap <MotorInfoUnit> motorInfoMap)
{
var res = new MotorInformation();
foreach (var m in motorParaMap.Keys)
{
res.PosInfo[m] = ConvertBackDistance(motorParaMap[m], motorInfoMap[m].Counter);
res.EncoderInfo[m] = ConvertBackDistance(motorParaMap[m], motorInfoMap[m].Encoder);
res.SpeedInfo[m] = ConvertBackSpeed(motorParaMap[m], motorInfoMap[m].Speed);
res.StatusInfo[m] = MotorStatus.FromRegContent((uint)motorInfoMap[m].Status);
}
return(res);
}
public bool DisableAmpC()
{
if (connected)
{
Status = MotorStatus.Disable;
bkWorker.CancelAsync();
enabled = false;
}
else
{
enabled = true;
}
return(!enabled);
}
public bool EnableAmpC()
{
if (connected)
{
enabled = true;
Status = MotorStatus.Enable;
bkWorker.RunWorkerAsync();
}
else
{
enabled = false;
}
return(enabled);
}
/// <summary>
/// 移动相对距离
/// </summary>
/// <param name="fPos"></param>
/// <returns></returns>
public bool JogInc(float fPos)
{
if (enabled)
{
startPosition = position; //将起始位置赋值为当前位置
targetPos = startPosition + fPos; //计算出运动的目标位置
ResetMoveValue(fPos);
Status = MotorStatus.JogMove;
if (!bkWorker.IsBusy)
{
stop = false;
bkWorker.RunWorkerAsync();
}
}
return(enabled);
}
void Awake()
{
//Reference
rb = GetComponent <Rigidbody2D>();
raycaster = GetComponent <MotorRaycaster>();
Feedbacks = GetComponentInChildren <Player2DFeedbacks>();
//Initialize various variables
status = new MotorStatus();
stateClassLookup = new Dictionary <MotorStates, MotorStateBase> //Store all the FSM classes using their common base class
{
{ MotorStates.OnGround, new MotorState_MoveOnGround(this, Feedbacks) },
{ MotorStates.Aerial, new MotorState_Aerial(this, Feedbacks) },
{ MotorStates.WallClimb, new MotorState_WallClimb(this, Feedbacks) },
{ MotorStates.Hurt, new MotorState_Hurt(this, Feedbacks) },
};
currentStateType = MotorStates.OnGround;
currentStateClass = stateClassLookup[currentStateType];
}
public bool InitAmpcStatus()
{
if (connected)
{
ampCFault = false;
pSoftLimit = false;
nSoftLimit = false;
pLimit = false;
nLimit = false;
inPosition = true;
homeSucessful = true;
Status = MotorStatus.Ready;
return(true);
}
else
{
Status = MotorStatus.DisConnect;
return(false);
}
}
[DllImport("EraeMotionApi.dll")] extern public static int ERAETech_EMCL_GetAllStatus2(int nPortNum, byte nModuleId, ref MotorStatus moReqField, ref MotorStatus moRetStatus);
public Motor(Door door)
{
this.door = door;
motorStatus = MotorStatus.STOPPED;
}
/// <summary>
/// This handler is called, whenever a message on the subscribed topic is received.
/// </summary>
/// <param name="message"> is the received message.</param>
protected override void ReceiveMessage(MotorStatus message)
{
Debug.Log("Received Message: " + message.id);
Debug.Log(message.position[0]);
}
private void buttonSaveClickedAsync()
{
try
{
Dictionary <int, int> dic = new Dictionary <int, int>();
MotorStatus motor1 = MotorStatus.Off;
MotorStatus motor2 = MotorStatus.Off;
if (!toggleButtonMotor1.Checked)
{
motor1 = MotorStatus.Off;
}
else if (radioButtonMotor1Speed1.Checked)
{
motor1 = MotorStatus.Speed1;
}
else if (radioButtonMotor1Speed2.Checked)
{
motor1 = MotorStatus.Speed2;
}
else if (radioButtonMotor1Speed3.Checked)
{
motor1 = MotorStatus.Speed3;
}
if (!toggleButtonMotor2.Checked)
{
motor2 = MotorStatus.Off;
}
else if (radioButtonMotor2Speed1.Checked)
{
motor2 = MotorStatus.Speed1;
}
else if (radioButtonMotor2Speed2.Checked)
{
motor2 = MotorStatus.Speed2;
}
else if (radioButtonMotor2Speed3.Checked)
{
motor2 = MotorStatus.Speed3;
}
SetStatusWrapper wrapper = new SetStatusWrapper();
wrapper.Motor1 = motor1;
wrapper.Motor2 = motor2;
var login = Login.GetValues(x => x.IsAuthorized).FirstOrDefault();
wrapper.SessionKey = login.SessionKey;
TechnicalFanCoil technicalFanCoil = new TechnicalFanCoil();
bool result = technicalFanCoil.SetStatus(wrapper);
ButtonRefreshClickedAsync();
//RunOnUiThread(() =>
//{
// buttonSave.Enabled = true;
//});
progressDialog.Dismiss();
}
catch (Exception ex)
{
//RunOnUiThread(() =>
//{
// buttonSave.Enabled = true;
// buttonRefresh.Enabled = true;
//});
progressDialog.Dismiss();
AlertDialog.Builder alert = new AlertDialog.Builder(this);
alert.SetTitle("Error");
alert.SetMessage(ex.Message);
alert.SetPositiveButton("Ok", delegate(object o, DialogClickEventArgs args)
{
return;
});
alert.Create().Show();
}
}
public MotorStatus get_motor_status(byte port)
{
//Read a motor status
//Keyword arguments:
//port -- The motor port (one at a time). PORT_A, PORT_B, PORT_C, or PORT_D.
//Returns a list:
// flags -- 8-bits of bit-flags that indicate motor status:
// bit 0 -- LOW_VOLTAGE_FLOAT - The motors are automatically disabled because the battery voltage is too low
// bit 1 -- OVERLOADED - The motors aren't close to the target (applies to position control and dps speed control).
// power -- the raw PWM power in percent (-100 to 100)
// encoder -- The encoder position
// dps -- The current speed in Degrees Per Second
MotorStatus motorStatus = new MotorStatus();
SPI_MESSAGE_TYPE message_type = SPI_MESSAGE_TYPE.NONE;
if (port == (byte)MOTOR_PORT.PORT_A)
{
message_type = SPI_MESSAGE_TYPE.GET_MOTOR_A_STATUS;
}
else if (port == (byte)MOTOR_PORT.PORT_B)
{
message_type = SPI_MESSAGE_TYPE.GET_MOTOR_B_STATUS;
}
else if (port == (byte)MOTOR_PORT.PORT_C)
{
message_type = SPI_MESSAGE_TYPE.GET_MOTOR_C_STATUS;
}
else if (port == (byte)MOTOR_PORT.PORT_D)
{
message_type = SPI_MESSAGE_TYPE.GET_MOTOR_D_STATUS;
}
else
{
throw new IOError("get_motor_status error. Must be one motor port at a time. PORT_A, PORT_B, PORT_C, or PORT_D.");
}
byte[] outArray = { SPI_Address, (byte)message_type, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
var reply = spi_transfer_array(outArray);
if (reply[3] == 0xA5)
{
motorStatus.Speed = reply[5];
if ((motorStatus.Speed & 0x80) > 0)
{
motorStatus.Speed = -motorStatus.Speed;
}
motorStatus.Encoder = (reply[6] << 24) | (reply[7] << 16) | (reply[8] << 8) | reply[9];
//negative should be managed
//if ((motorStatus.Encoder & 0x80000000) > 0)
// motorStatus.Encoder = motorStatus.Encoder - 0x100000000;
motorStatus.Dps = ((reply[10] << 8) | reply[11]);
if ((motorStatus.Dps & 0x8000) > 0)
{
motorStatus.Dps = motorStatus.Dps - 0x10000;
}
motorStatus.Flags = (MotorStatusFlags)reply[4];
}
else
{
throw new IOError("No SPI response");
}
return(motorStatus);
}
public void SetMotorStatus(MotorStatus motorStatus)
{
this.motorStatus = motorStatus;
}
[DllImport("EraeMotionApi.dll", CallingConvention = CallingConvention.Cdecl)] extern public static int ERAETech_EMCL_GetAllStatus(int nPortNum, byte nModuleId, ref MotorStatus moStatus);
void registerMotorStatus (MotorStatus motor_status)
{
motor_status_ = new MotorStatusHandle ( this, motor_status );
}
