/// <summary>
/// Updates the motors on the manipulator in mix and match mode. Called every frame.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="dioModules"></param>
/// <param name="controlIndex"></param>
public static void UpdateManipulatorMotors(RigidNode_Base skeleton, UnityPacket.OutputStatePacket.DIOModule[] dioModules, int controlIndex)
{
float[] pwm;
float[] can;
if (dioModules[0] != null)
{
pwm = dioModules[0].pwmValues;
can = dioModules[0].canValues;
}
else
{
pwm = new float[10];
can = new float[10];
}
pwm[4] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm4Axes) * SpeedArrowPwm);
pwm[5] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm5Axes) * SpeedArrowPwm);
pwm[6] +=
(InputControl.GetAxis(Controls.axes[controlIndex].pwm6Axes) * SpeedArrowPwm);
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
foreach (RigidNode_Base node in listOfSubNodes)
{
RigidNode rigidNode = (RigidNode)node;
BRaycastWheel raycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
SkeletalJoint_Base joint = rigidNode.GetSkeletalJoint();
if (raycastWheel != null)
{
if (joint.cDriver.port1 == i + 1)
{
float output = pwm[i];
MotorType motorType = joint.cDriver.GetMotorType();
float torque = motorType == MotorType.GENERIC ? 2.42f : 60 * motorDefinition[motorType].baseTorque - motorDefinition[motorType].slope * raycastWheel.GetWheelSpeed() / 9.549297f;
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
torque /= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
}
raycastWheel.ApplyForce(output, torque, motorType == MotorType.GENERIC);
}
}
if (joint != null && joint.cDriver != null)
{
if (joint.cDriver.GetDriveType().IsMotor() && rigidNode.MainObject.GetComponent <BHingedConstraint>() != null)
{
if (joint.cDriver.port1 == i + 1)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
friction = HingeCostFriction;
MotorType motorType = joint.cDriver.GetMotorType();
Motor motor = motorType == MotorType.GENERIC ? new Motor(10f, 4f) : motorDefinition[motorType];
maxSpeed = motor.maxSpeed;
impulse = motor.baseTorque - motor.slope * ((RigidBody)(rigidNode.MainObject.GetComponent <BRigidBody>().GetCollisionObject())).AngularVelocity.Length / 9.549297f;
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
float gearRatio = Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
impulse /= gearRatio;
maxSpeed *= gearRatio;
}
BHingedConstraint hingedConstraint = rigidNode.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = pwm[i] > 0f ? maxSpeed : pwm[i] < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = joint.cDriver.hasBrake ? motor.baseTorque : pwm[i] == 0f ? friction : Mathf.Abs(pwm[i] * impulse);
}
}
else if (joint.cDriver.GetDriveType().IsElevator())
{
if (joint.cDriver.port1 == i + 1 && rigidNode.HasDriverMeta <ElevatorDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MaxSliderForce;
sc.TargetLinearMotorVelocity = pwm[i] * MaxSliderSpeed;
}
}
else if (joint.cDriver.GetDriveType().IsPneumatic() && rigidNode.HasDriverMeta <PneumaticDriverMeta>())
{
BSliderConstraint bSliderConstraint = rigidNode.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
float output = motors[joint.cDriver.port1 - 1];
float psi = node.GetDriverMeta <PneumaticDriverMeta>().pressurePSI * 6894.76f;
float width = node.GetDriverMeta <PneumaticDriverMeta>().widthMM * 0.001f;
float stroke = (sc.UpperLinearLimit - sc.LowerLinearLimit) / 0.01f;
float force = psi * ((float)Math.PI) * width * width / 4f;
float speed = stroke / 60f;
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = force;
sc.TargetLinearMotorVelocity = sc.TargetLinearMotorVelocity != 0 && output == 0 ? sc.TargetLinearMotorVelocity : output * speed;
}
}
}
}
}
/// <summary>
/// Updates all motor values from the given <see cref="RigidNode_Base"/>, pwm values, and emulation network info.
/// </summary>
/// <param name="skeleton"></param>
/// <param name="pwm"></param>
/// <param name="emuList"></param>
public static void UpdateAllMotors(RigidNode_Base skeleton, float[] pwm, List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList)
{
listOfSubNodes.Clear();
skeleton.ListAllNodes(listOfSubNodes);
for (int i = 0; i < pwm.Length; i++)
{
motors[i] = pwm[i];
}
if (Synthesis.GUI.EmulationDriverStation.Instance != null)
{
UpdateEmulationJoysticks();
UpdateEmulationMotors(pwm);
UpdateEmulationSensors(emuList);
}
foreach (RigidNode node in listOfSubNodes.Select(n => n as RigidNode))
{
SkeletalJoint_Base joint = node.GetSkeletalJoint();
if (joint == null || joint.cDriver == null)
{
continue;
}
BRaycastWheel raycastWheel = node.MainObject.GetComponent <BRaycastWheel>();
if (raycastWheel != null)
{
float output = motors[node.GetSkeletalJoint().cDriver.port1 - 1];
MotorType motorType = joint.cDriver.GetMotorType();
float torque = motorType == MotorType.GENERIC ? 2.42f : 60 * motorDefinition[motorType].baseTorque - motorDefinition[motorType].slope * raycastWheel.GetWheelSpeed() / 9.549297f;
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
torque /= Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
}
raycastWheel.ApplyForce(output, torque, motorType == MotorType.GENERIC);
}
else if (joint.cDriver.GetDriveType().IsMotor() && node.MainObject.GetComponent <BHingedConstraint>() != null)
{
float maxSpeed = 0f;
float impulse = 0f;
float friction = 0f;
float output = !joint.cDriver.isCan ? motors[joint.cDriver.port1 - 1] : motors[joint.cDriver.port1 - 10];
friction = HingeCostFriction;
MotorType motorType = joint.cDriver.GetMotorType();
Motor motor = motorType == MotorType.GENERIC ? new Motor(10f, 4f) : motorDefinition[motorType];
maxSpeed = motor.maxSpeed;
impulse = motor.baseTorque - motor.slope * ((RigidBody)(node.MainObject.GetComponent <BRigidBody>().GetCollisionObject())).AngularVelocity.Length / 9.549297f;
if (joint.cDriver.InputGear != 0 && joint.cDriver.OutputGear != 0)
{
float gearRatio = Convert.ToSingle(joint.cDriver.InputGear / joint.cDriver.OutputGear);
impulse /= gearRatio;
maxSpeed *= gearRatio;
}
BHingedConstraint hingedConstraint = node.MainObject.GetComponent <BHingedConstraint>();
hingedConstraint.enableMotor = true;
hingedConstraint.targetMotorAngularVelocity = output > 0f ? maxSpeed : output < 0f ? -maxSpeed : 0f;
hingedConstraint.maxMotorImpulse = node.GetSkeletalJoint().cDriver.hasBrake ? motor.baseTorque : output == 0f ? friction : Mathf.Abs(output * impulse);
}
else if (joint.cDriver.GetDriveType().IsElevator() && node.HasDriverMeta <ElevatorDriverMeta>())
{
float output = motors[joint.cDriver.port1 - 1];
BSliderConstraint bSliderConstraint = node.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = MaxSliderForce;
sc.TargetLinearMotorVelocity = output * MaxSliderSpeed;
}
else if (joint.cDriver.GetDriveType().IsPneumatic() && node.HasDriverMeta <PneumaticDriverMeta>())
{
BSliderConstraint bSliderConstraint = node.MainObject.GetComponent <BSliderConstraint>();
SliderConstraint sc = (SliderConstraint)bSliderConstraint.GetConstraint();
float output = motors[joint.cDriver.port1 - 1];
float psi = node.GetDriverMeta <PneumaticDriverMeta>().pressurePSI * 6894.76f;
float width = node.GetDriverMeta <PneumaticDriverMeta>().widthMM * 0.001f;
float stroke = (sc.UpperLinearLimit - sc.LowerLinearLimit) / 0.01f;
float force = psi * ((float)Math.PI) * width * width / 4f;
float speed = stroke / 60f;
sc.PoweredLinearMotor = true;
sc.MaxLinearMotorForce = force;
sc.TargetLinearMotorVelocity = sc.TargetLinearMotorVelocity != 0 && output == 0 ? sc.TargetLinearMotorVelocity : output * speed;
}
}
}
/// <summary>
/// Updates all emulation sensor values.
/// </summary>
/// <param name="emuList"></param>
private static void UpdateEmulationSensors(List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList)
{
int iter = 0;
foreach (Synthesis.Robot.RobotBase.EmuNetworkInfo a in emuList)
{
RigidNode rigidNode = null;
try
{
rigidNode = (RigidNode)(a.wheel);
}
catch (Exception e)
{
Debug.Log(e.StackTrace);
}
BRaycastWheel bRaycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
if (a.RobotSensor.type == RobotSensorType.ENCODER)
{
//BRaycastRobot robot = rigidNode.MainObject.GetComponent<BRaycastRobot>();
double wheelRadius = 3 / 39.3701;// robot.RaycastRobot.GetWheelInfo(0).WheelsRadius;
Vector3 currentPos = bRaycastWheel.transform.position;
//double displacement = (Math.Sqrt(currentPos.x * currentPos.x + currentPos.z * currentPos.z) - (Math.Sqrt(a.previousPosition.x*a.previousPosition.x + a.previousPosition.z * a.previousPosition.z)));
double displacement = ((currentPos - a.previousPosition).magnitude) * Math.Sign(bRaycastWheel.GetWheelSpeed());
double angleDisplacement = (displacement) / (2 * 3.1415 * wheelRadius);
a.encoderTickCount += angleDisplacement * a.RobotSensor.conversionFactor;
a.previousPosition = currentPos;
var portAType = a.RobotSensor.conTypePortA.ToString() == "DIO" ? "DI" : a.RobotSensor.conTypePortA.ToString();
var portBType = a.RobotSensor.conTypePortB.ToString() == "DIO" ? "DI" : a.RobotSensor.conTypePortB.ToString();
if (InputManager.Instance.Roborio.Encoders[iter] == null)
{
InputManager.Instance.Roborio.Encoders[iter] = new EncoderData();
}
if (Synthesis.GUI.EmulationDriverStation.Instance.isRunCode)
{
InputManager.Instance.Roborio.Encoders[iter].updateEncoder((int)a.RobotSensor.portA, portAType, (int)a.RobotSensor.portB, portBType, (int)a.encoderTickCount);
}
iter++;
}
}
}
/// <summary>
/// Updates all emulation sensor values.
/// </summary>
/// <param name="emuList"></param>
private static void UpdateEmulationSensors(List <Synthesis.Robot.RobotBase.EmuNetworkInfo> emuList)
{
int iter = 0;
foreach (Synthesis.Robot.RobotBase.EmuNetworkInfo a in emuList)
{
if (a.RobotSensor.type == RobotSensorType.ENCODER) // TODO revisit this
{
RigidNode rigidNode = null;
try
{
rigidNode = (RigidNode)(a.wheel);
}
catch (Exception e)
{
Debug.Log(e.StackTrace);
}
BRaycastWheel bRaycastWheel = rigidNode.MainObject.GetComponent <BRaycastWheel>();
//BRaycastRobot robot = rigidNode.MainObject.GetComponent<BRaycastRobot>();
double wheelRadius = 3 / 39.3701;// robot.RaycastRobot.GetWheelInfo(0).WheelsRadius
Vector3 currentPos = bRaycastWheel.transform.position;
//double displacement = (Math.Sqrt(currentPos.x * currentPos.x + currentPos.z * currentPos.z) - (Math.Sqrt(a.previousPosition.x*a.previousPosition.x + a.previousPosition.z * a.previousPosition.z)));
double displacement = ((currentPos - a.previousPosition).magnitude) * Math.Sign(bRaycastWheel.GetWheelSpeed());
double angleDisplacement = (displacement) / (2 * 3.1415 * wheelRadius);
a.encoderTickCount += angleDisplacement * a.RobotSensor.conversionFactor;
a.previousPosition = currentPos;
if (Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter] == null)
{
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter] = new EmulationService.RobotInputs.Types.EncoderManager();
}
EmulationService.RobotInputs.Types.EncoderManager.Types.PortType ConvertPortType(SensorConnectionType type)
{
if (type == SensorConnectionType.DIO)
{
return(EmulationService.RobotInputs.Types.EncoderManager.Types.PortType.Di);
}
if (type == SensorConnectionType.ANALOG)
{
return(EmulationService.RobotInputs.Types.EncoderManager.Types.PortType.Ai);
}
throw new Exception();
}
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].AChannel = (uint)a.RobotSensor.portA;
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].AType = ConvertPortType(a.RobotSensor.conTypePortA);;
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].BChannel = (uint)a.RobotSensor.portB;
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].BType = ConvertPortType(a.RobotSensor.conTypePortB);;
Synthesis.EmulatedRoboRIO.RobotInputs.EncoderManagers[iter].Ticks = (int)a.encoderTickCount;
iter++;
}
}
}
