private void UpdateDrill(NicbotBodyDrillContext drillContext, double elapsedSeconds)
{
if ((drillContext.control & 0x01) != 0)
{
drillContext.OnLaser(true);
}
else
{
drillContext.OnLaser(false);
}
if (NicbotBodyDrillStates.idle == drillContext.state)
{
if ((drillContext.control & 0x04) != 0)
{
drillContext.control &= 0xFB;
this.SetServoAcceleration(drillContext.axis, this.drillServoAcceleration);
this.SetServoVelocity(drillContext.axis, this.drillServoHomingVelocity);
this.SetServoPosition(drillContext.axis, 20000000);
this.MoveServoRelative(drillContext.axis);
this.SendDebug(0xBC, 1);
drillContext.state = NicbotBodyDrillStates.homeRetractToLimit;
}
else if (drillContext.manualSetPoint != drillContext.processedSetPoint)
{
Int32 positionRequest;
positionRequest = -drillContext.manualSetPoint;
positionRequest *= (Int32)drillServoPulsesPerUnit;
positionRequest /= 100;
positionRequest--;
this.SetServoAcceleration(drillContext.axis, this.drillServoAcceleration);
this.SetServoVelocity(drillContext.axis, this.drillServoTravelVelocity);
this.SetServoPosition(drillContext.axis, positionRequest);
this.MoveServoAbsolute(drillContext.axis);
drillContext.processedSetPoint = drillContext.manualSetPoint;
this.SendDebug(0xBC, 10);
drillContext.state = NicbotBodyDrillStates.manual;
}
}
else
{
byte servoStatus;
servoStatus = this.GetServoStatus(drillContext.axis);
if ((servoStatus & 0x20) != 0)
{
drillContext.state = NicbotBodyDrillStates.faulted;
//this.devicefasetDeviceFault(servoExcessivePositionFaultCode);
}
else if ((drillContext.control & 0x02) != 0)
{
drillContext.control &= 0xFD;
this.StopServo(drillContext.axis);
this.SendDebug(0xBC, 12);
drillContext.state = NicbotBodyDrillStates.stop;
}
else
{
if (NicbotBodyDrillStates.manual == drillContext.state)
{
// todo check limits to stop
if ((servoStatus & 0x04) != 0)
{
this.SendDebug(0xBC, 11);
drillContext.state = NicbotBodyDrillStates.idle;
}
else if (drillContext.manualSetPoint != drillContext.processedSetPoint)
{
Int32 positionRequest;
positionRequest = -drillContext.manualSetPoint;
positionRequest *= (Int32)drillServoPulsesPerUnit;
positionRequest /= 100;
positionRequest--;
this.SendDebug(0xCD, (UInt32)positionRequest);
this.SetServoPosition(drillContext.axis, positionRequest);
this.MoveServoAbsoluteWhileMoving(drillContext.axis);
drillContext.processedSetPoint = drillContext.manualSetPoint;
}
}
else if ((NicbotBodyDrillStates.stop == drillContext.state))
{
if ((servoStatus & 0x04) != 0)
{
this.SendDebug(0xBC, 13);
drillContext.state = NicbotBodyDrillStates.idle;
}
}
else if ((NicbotBodyDrillStates.homeRetractToLimit == drillContext.state))
{
if (((this.StatusWord & drillContext.retractMask) != 0))
{
this.StopServo(drillContext.axis);
this.SendDebug(0xBC, 2);
drillContext.state = NicbotBodyDrillStates.homeStopFromRetract;
}
}
else if ((NicbotBodyDrillStates.homeStopFromRetract == drillContext.state))
{
if ((servoStatus & 0x04) != 0)
{
this.SetServoPosition(drillContext.axis, -4000000);
this.MoveServoRelative(drillContext.axis);
this.SendDebug(0xBC, 3);
drillContext.state = NicbotBodyDrillStates.homeExtendToNotLimit;
}
}
else if ((NicbotBodyDrillStates.homeExtendToNotLimit == drillContext.state))
{
if (((this.StatusWord & drillContext.retractMask) == 0))
{
this.StopServo(drillContext.axis);
this.SendDebug(0xBC, 4);
drillContext.state = NicbotBodyDrillStates.homeStopFromExtend;
}
}
else if ((NicbotBodyDrillStates.homeStopFromExtend == drillContext.state))
{
if ((servoStatus & 0x04) != 0)
{
Int32 positionRequest = (Int32)this.drillServoHomingBackoffCount;
positionRequest *= -1;
this.SetServoPosition(drillContext.axis, positionRequest);
this.MoveServoRelative(drillContext.axis);
this.SendDebug(0xBC, 5);
drillContext.state = NicbotBodyDrillStates.homeBackoff;
}
}
else if ((NicbotBodyDrillStates.homeBackoff == drillContext.state))
{
if ((servoStatus & 0x04) != 0)
{
this.SetServoOrigin(drillContext.axis);
drillContext.processedSetPoint = 0;
drillContext.manualSetPoint = 0;
this.SendDebug(0xBC, 6);
drillContext.state = NicbotBodyDrillStates.idle;
}
}
}
}
}
public UlcRoboticsNicbotBody()
: base()
{
this.InitializeComponent();
this.randomValue = new Random();
this.deviceType = 0x00003010;
this.errorStatus = 0;
this.deviceName = "NICBOT Body";
this.version = "v1.00";
this.nvBaudRateCode = 3;
this.nvNodeId = 32;
this.tpdoMapping = new TPDOMapping[4];
for (int i = 0; i < 4; i++)
{
this.tpdoMapping[i] = new TPDOMapping();
this.tpdoMapping[i].OnPdoMappable = new TPDOMapping.PdoMappableHandler(this.TPdoMappableHandler);
this.tpdoMapping[i].OnPdoSize = new TPDOMapping.PdoSizeHandler(this.TPdoSizeHandler);
this.tpdoMapping[i].OnPdoData = new TPDOMapping.PdoDataHandler(this.TPdoDataHandler);
}
this.NodeIdTextBox.Text = this.nvNodeId.ToString();
this.frontDrillContext = new NicbotBodyDrillContext();
this.frontDrillContext.OnLaser = new NicbotBodyDrillContext.LaserHandler(this.FrontLaserControl);
this.frontDrillContext.retractMask = 0x0400;
this.frontDrillContext.axis = 1;
this.rearDrillContext = new NicbotBodyDrillContext();
this.rearDrillContext.OnLaser = new NicbotBodyDrillContext.LaserHandler(this.RearLaserControl);
this.rearDrillContext.retractMask = 0x0100;
this.rearDrillContext.axis = 0;
}
