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; }