void FixedUpdate()
{
// if we don't have a simMotor assigned to this component, don't
// do anything
if (simMotor != null)
{
// find the motor we look to for commands
SimMotor leader = simMotor;
if (simMotor.Following != null)
{
leader = simMotor.Following;
}
double radsPerSecond = leader.Config.TargetVelocity;
double radsPosition = leader.Config.TargetPosition;
switch (leader.Config.ControlMode)
{
case MotorConfig.Types.ControlMode.PercentOutput:
float output = (float)(stallTorque * leader.Config.TargetOutput);
body.AddRelativeTorque(0, output, 0);
break;
case MotorConfig.Types.ControlMode.MotionVelocity:
case MotorConfig.Types.ControlMode.Velocity:
break;
case MotorConfig.Types.ControlMode.Position:
break;
}
}
}
/// <summary>
/// This will run a motor through a pid loop to determine the output
/// </summary>
/// <param name="motor"></param>
/// <param name="setPoint"></param>
/// <param name="pv"></param>
/// <returns></returns>
double RunPid(SimMotor motor, double setPoint, double pv)
{
MotorConfig.Types.FPID fpid = motor.Config.Fpid[motor.Config.CurrentPidProfile];
// calculate p
double error = setPoint - pv;
double pComp = error * fpid.P;
// calculate the (iState or integralState)
if (Math.Abs(error) <= fpid.IZone || fpid.IZone == 0.0f)
{
motor.IntegralState = motor.IntegralState + (error * fpid.I);
}
else
{
motor.IntegralState = 0;
}
// calculate the d component
double dComp = (error - motor.LastError);
motor.LastError = error;
dComp *= fpid.D;
// calculate the f component
double fComp = setPoint * fpid.F;
// for a PID controller, the output is the sum of the components
double output = pComp + motor.IntegralState + dComp + fComp;
return(output);
}
// Start is called before the first frame update
void Start()
{
EventManager.PublishMotorConfigUpdateEvent(new FRCSim.MotorConfig()
{
Id = 1,
Handle = 100001,
});
EventManager.PublishMotorConfigUpdateEvent(new FRCSim.MotorConfig()
{
Id = 2,
Handle = 100002,
FollowingId = 1
});
EventManager.PublishMotorConfigUpdateEvent(new FRCSim.MotorConfig()
{
Id = 3,
Handle = 100003,
});
EventManager.PublishMotorConfigUpdateEvent(new FRCSim.MotorConfig()
{
Id = 4,
Handle = 100004,
FollowingId = 3
});
leftFrontMotor = motorStore.GetSimMotor(100001);
leftRearMotor = motorStore.GetSimMotor(100002);
rightFrontMotor = motorStore.GetSimMotor(100003);
rightRearMotor = motorStore.GetSimMotor(100004);
}
private void MotorConfigUpdated(MotorConfig motorConfig)
{
SimMotor simMotor;
if (!simMotorsByHandle.TryGetValue(motorConfig.Handle, out simMotor))
{
simMotor = new SimMotor()
{
Id = motorConfig.Id,
Handle = motorConfig.Handle
};
simMotors.Add(simMotor);
simMotorsByHandle.Add(simMotor.Handle, simMotor);
simMotorsById.Add(simMotor.Id, simMotor);
}
simMotor.Config = motorConfig;
if (motorConfig.FollowingId != 0)
{
SimMotor following;
if (simMotorsById.TryGetValue(motorConfig.FollowingId, out following))
{
simMotor.Following = following;
}
}
else
{
simMotor.Following = null;
}
}
/// <summary>
/// When this component gets a motor update, if it's the motor assigned
/// to this component, update the SimMotor object
/// </summary>
/// <param name="motorConfig"></param>
private void MotorConfigUpdated(MotorConfig motorConfig)
{
if (motorId != motorConfig.Id)
{
// this motor isn't assigned to us, ignore the update
return;
}
simMotor = motorStore.CreateOrUpdateMotor(motorConfig);
}
public SimMotor CreateOrUpdateMotor(MotorConfig motorConfig)
{
SimMotor simMotor;
if (!simMotorsByHandle.TryGetValue(motorConfig.Handle, out simMotor))
{
simMotor = new SimMotor()
{
Id = motorConfig.Id,
Handle = motorConfig.Handle
};
// initialize the MotorOutput
simMotors.Add(simMotor);
simMotorsByHandle.Add(simMotor.Handle, simMotor);
simMotorsById.Add(simMotor.Id, simMotor);
Debug.Log("Created motor: " + simMotor.Id);
}
simMotor.Config = motorConfig;
if (motorConfig.FollowingId != 0 && simMotor.Following == null)
{
SimMotor following;
if (simMotorsById.TryGetValue(motorConfig.FollowingId, out following))
{
simMotor.Following = following;
Debug.Log("motor " + motorConfig + " following " + following.Config);
}
}
else
{
simMotor.Following = null;
}
return(simMotor);
}
