private bool fastMove(motorBase motor)
{
TimeSpan ts = befTime - DateTime.UtcNow;
double diffAngle = MathHelperD.ToDegrees(motor.targetRadians) - MathHelperD.ToDegrees(motor.motorRadian);
double rad = MathHelperD.ToRadians(diffAngle) / (-0.03);
if (targetRPM < Math.Abs(rad * 9.549))
{
MyMotor.TargetVelocityRPM = MathHelperD.ToDegrees(rad) < 0 ? targetRPM : -targetRPM;
}
else
{
motor.MyMotor.TargetVelocityRad = -(float)(rad / 2);
}
befTime = DateTime.UtcNow;
motor.befRadian = motor.motorRadian;
if (Math.Abs(MathHelperD.ToDegrees(motor.motorRadian) - MathHelperD.ToDegrees(motor.targetRadians)) < 0.5)
{
motor.MyMotor.TargetVelocityRad = 0;
return(true);
}
else
{
return(false);
}
}
private bool justMove(motorBase motor)
{
TimeSpan ts = befTime - nowTime;
double ang = (motor.Angle - motor.BefAngle);
double diffAngle = motor.TargetAngle - motor.Angle;
double rad = MathHelperD.ToRadians(diffAngle) / ts.TotalSeconds;
if (diffAngle > rad)
{
motor.MyMotor.TargetVelocityRPM = motor.TargetRPM;
}
else
{
motor.MyMotor.TargetVelocityRPM = (float)(diffAngle / (Math.PI * 2));
}
motor.BefAngle = motor.Angle;
if ((motor.Angle - motor.TargetAngle) < 0.5)
{
return(true);
}
else
{
return(false);
}
}
private bool justMove(motorBase motor)
{
TimeSpan ts = befTime - nowTime;
double ang = (MathHelperD.ToDegrees(motor.motorRadian) - MathHelperD.ToDegrees(motor.befRadian));
double diffAngle = MathHelperD.ToDegrees(motor.targetRadians) - MathHelperD.ToDegrees(motor.motorRadian);
double rad = MathHelperD.ToRadians(diffAngle) / ts.TotalSeconds;
if (diffAngle > rad)
{
motor.MyMotor.TargetVelocityRPM = motor.targetRPM;
}
else
{
motor.MyMotor.TargetVelocityRPM = (float)(diffAngle / (Math.PI * 2));
}
motor.befRadian = motor.motorRadian;
if ((MathHelperD.ToDegrees(motor.motorRadian) - MathHelperD.ToDegrees(motor.targetRadians)) < 0.5)
{
return(true);
}
else
{
return(false);
}
}
private bool fastMove(motorBase motor)
{
TimeSpan ts = befTime - DateTime.UtcNow;
//double ang = (motor.Angle - motor.BefAngle);
double diffAngle = motor.TargetAngle - motor.Angle;
double rad = MathHelperD.ToRadians(diffAngle) / (-0.03);
// motor.MyMotor.TargetVelocityRad = -(float)(rad / 5);
if (TargetRPM < Math.Abs(rad * 9.549))
{
MyMotor.TargetVelocityRPM = MathHelperD.ToDegrees(rad) < 0 ? TargetRPM : -TargetRPM;
}
else
{
motor.MyMotor.TargetVelocityRad = -(float)(rad / 2);
}
befTime = DateTime.UtcNow;
motor.BefAngle = motor.Angle;
if (Math.Abs(motor.Angle - motor.TargetAngle) < 0.5)
{
motor.MyMotor.TargetVelocityRad = 0;
return(true);
}
else
{
return(false);
}
}
private bool fastMove(motorBase motor)
{
TimeSpan ts = befTime - DateTime.UtcNow;
//double ang = (motor.Angle - motor.BefAngle);
double diffAngle = motor.TargetAngle - motor.Angle;
double rad = MathHelperD.ToRadians(diffAngle) / (-0.02);
motor.MyMotor.TargetVelocityRad = -(float)(rad / 5);
befTime = DateTime.UtcNow;
motor.BefAngle = motor.Angle;
if (Math.Abs(motor.Angle - motor.TargetAngle) < 0.5)
{
motor.MyMotor.TargetVelocityRad = 0;
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// コンストラクタ
/// </summary>
/// <param name="m1">第１関節</param>
/// <param name="r1">第１関節反転フラグ</param>
/// <param name="m2">第２関節</param>
/// <param name="r2">第２関節反転フラグ</param>
/// <param name="m3">第３関節</param>
/// <param name="r3">第３関節反転フラグ</param>
public LegBase(IMyMotorStator m1, bool r1, IMyMotorStator m2, bool r2, IMyMotorStator m3, bool r3)
{
LegStatus = Utilty.PartsMoveEnum.off;
myMotorThigh = new motorBase(m1, r1, 0, 0);
myMotorKnee = new motorBase(m2, r2, 0, 0);
myMotorAnkle = new motorBase(m3, r3, 0, 0);
}
/// <summary>
/// コンストラクタ
/// </summary>
/// <param name="m1">第１関節</param>
/// <param name="r1">第１関節反転フラグ</param>
/// <param name="m2">第２関節</param>
/// <param name="r2">第２関節反転フラグ</param>
/// <param name="m3">第３関節</param>
/// <param name="r3">第３関節反転フラグ</param>
public ArmBase(IMyMotorStator m1, bool r1, IMyMotorStator m2, bool r2, IMyMotorStator m3, bool r3, IMyMotorStator m4, bool r4)
{
LegStatus = Utilty.PartsMoveEnum.off;
myMotor1 = new motorBase(m1, r1, 0, 0);
myMotor2 = new motorBase(m2, r2, 0, 0);
myMotor3 = new motorBase(m3, r3, 0, 0);
myMotor4 = new motorBase(m4, r4, 0, 0);
}
public Program()
{
// The constructor, called only once every session and
// always before any other method is called. Use it to
// initialize your script.
//
// The constructor is optional and can be removed if not
// needed.
//
// It's recommended to set RuntimeInfo.UpdateFrequency
// here, which will allow your script to run itself without a
// timer block.
MyMotorBase = GridTerminalSystem.GetBlockWithName("Rotor") as IMyMotorStator;
motor = new motorBase(MyMotorBase, false, 0, 0);
MyMotorBase.CustomData = "0,0,0";
Runtime.UpdateFrequency = UpdateFrequency.Update1;
}
