Пример #1
0
      private void UpdateRobotBody()
      {
         bool evaluateSolenoidSetPoint = false;
         UInt16 solenoidSetPoint = 0;

         if (this.videoASetPoint != this.videoARequested)
         {
            this.robotBody.SetVideoASelect((byte)this.videoASetPoint);
            this.videoARequested = this.videoASetPoint;
         }

         if (this.videoBSetPoint != this.videoBRequested)
         {
            this.robotBody.SetVideoBSelect((byte)this.videoBSetPoint);
            this.videoBRequested = this.videoBSetPoint;
         }

         for (int i = 0; i < this.cameraLightLevelSetPoints.Length; i++)
         {
            if (this.cameraLightLevelSetPoints[i] != this.cameraLightLevelRequests[i])
            {
               int level = (255 * this.cameraLightLevelSetPoints[i]) / 100;
               this.robotBody.SetCameraLightLevelt((byte)(i + 1), (byte)level);

               this.cameraLightLevelRequests[i] = this.cameraLightLevelSetPoints[i];
            }
         }

         this.movementWheelModeActual = this.movementWheelModeChangeRequest;

         lock (this)
         {
            if (0 == this.robotSolenoidSetPointChangeCounter)
            {
               evaluateSolenoidSetPoint = true;
               solenoidSetPoint = this.robotSolenoidSetPoint;
            }
         }

         if ((false != evaluateSolenoidSetPoint) &&
             (solenoidSetPoint != this.robotSolenoidRequested))
         {
            this.robotBody.SetSolenoids(solenoidSetPoint);
            this.robotSolenoidRequested = solenoidSetPoint;
         }

         if (RobotApplications.repair == ParameterAccessor.Instance.RobotApplication)
         {
            if (this.drillFrontRotationSetPoint != this.drillFrontRotationRequested)
            {

               this.robotBody.SetFrontDrillSpeed(this.drillFrontRotationSetPoint);
               this.drillFrontRotationRequested = this.drillFrontRotationSetPoint;
            }

            if (this.drillRearRotationSetPoint != this.drillRearRotationRequested)
            {
               this.robotBody.SetRearDrillSpeed(this.drillRearRotationSetPoint);
               this.drillRearRotationRequested = this.drillRearRotationSetPoint;
            }

            if (false != this.drillFrontConfigurationNeeded)
            {
               this.drillFrontIndexSpeedRequested = 0;
               this.robotBody.SetDrillServoAcceleration((UInt32)ParameterAccessor.Instance.FrontDrill.Acceleration.OperationalValue);
               this.robotBody.SetDrillServoErrorLimit((UInt16)ParameterAccessor.Instance.FrontDrill.ErrorLimit.OperationalValue);
               this.robotBody.SetDrillServoProportionalControlConstant((UInt32)ParameterAccessor.Instance.FrontDrill.ProportionalControlConstant.OperationalValue);
               this.robotBody.SetDrillServoIntegralControlConstant((UInt32)ParameterAccessor.Instance.FrontDrill.IntegralControlConstant.OperationalValue);
               this.robotBody.SetDrillServoDerivativeControlConstant((UInt32)ParameterAccessor.Instance.FrontDrill.DerivativeControlConstant.OperationalValue);
               this.drillFrontConfigurationNeeded = false;
            }

            if (false != this.drillRearConfigurationNeeded)
            {
               this.drillRearIndexSpeedRequested = 0;
               this.robotBody.SetDrillServoAcceleration((UInt32)ParameterAccessor.Instance.RearDrill.Acceleration.OperationalValue);
               this.robotBody.SetDrillServoErrorLimit((UInt16)ParameterAccessor.Instance.RearDrill.ErrorLimit.OperationalValue);
               this.robotBody.SetDrillServoProportionalControlConstant((UInt32)ParameterAccessor.Instance.RearDrill.ProportionalControlConstant.OperationalValue);
               this.robotBody.SetDrillServoIntegralControlConstant((UInt32)ParameterAccessor.Instance.RearDrill.IntegralControlConstant.OperationalValue);
               this.robotBody.SetDrillServoDerivativeControlConstant((UInt32)ParameterAccessor.Instance.RearDrill.DerivativeControlConstant.OperationalValue);
               this.drillRearConfigurationNeeded = false;
            }

            if (false != this.drillFrontRetractToLimit)
            {
               this.robotBody.SetFrontDrillRetractToLimit();
               this.drillFrontRetractToLimit = false;
            }

            if (false != this.drillRearRetractToLimit) 
            {
               this.robotBody.SetRearDrillRetractToLimit();
               this.drillRearRetractToLimit = false;
            }

            if (false != this.drillFrontStop)
            {
               this.robotBody.StopFrontDrill();
               this.drillFrontStop = false;
            }

            if (false != this.drillRearStop)
            {
               this.robotBody.StopRearDrill();
               this.drillRearStop = false;
            }

            if (this.drillFrontIndexSetPoint != this.drillFrontIndexRequested)
            {
               double indexerSpeed = 0;

               if (double.IsNaN(this.drillFrontOriginOffset) == false)
               {
                  if (this.drillFrontIndexSetPoint > this.drillFrontOriginOffset)
                  {
                     indexerSpeed = ParameterAccessor.Instance.FrontDrill.CuttingSpeed.OperationalValue;
                  }
                  else
                  {
                     indexerSpeed = ParameterAccessor.Instance.FrontDrill.TravelSpeed.OperationalValue;
                  }
               }
               else
               {
                  indexerSpeed = ParameterAccessor.Instance.FrontDrill.SearchSpeed.OperationalValue;
               }

               if (indexerSpeed != this.drillFrontIndexSpeedRequested)
               {
                  UInt32 velocityCount = (UInt32)(indexerSpeed * ParameterAccessor.Instance.FrontDrill.SpeedToVelocityCount);
                  this.robotBody.SetDrillServoTravelVelocity(velocityCount);
                  this.drillFrontIndexSpeedRequested = indexerSpeed;
               }

               this.robotBody.SetFrontDrillIndex(this.drillFrontIndexSetPoint);
               this.drillFrontIndexRequested = this.drillFrontIndexSetPoint;
            }

            if (this.drillRearIndexSetPoint != this.drillRearIndexRequested)
            {
               double indexerSpeed = 0;

               if (double.IsNaN(this.drillRearOriginOffset) == false)
               {
                  if (this.drillRearOriginOffset > this.drillRearIndexSetPoint)
                  {
                     indexerSpeed = ParameterAccessor.Instance.RearDrill.CuttingSpeed.OperationalValue;
                  }
                  else
                  {
                     indexerSpeed = ParameterAccessor.Instance.RearDrill.TravelSpeed.OperationalValue;
                  }
               }
               else
               {
                  indexerSpeed = ParameterAccessor.Instance.RearDrill.SearchSpeed.OperationalValue;
               }

               if (indexerSpeed != this.drillRearIndexSpeedRequested)
               {
                  UInt32 velocityCount = (UInt32)(indexerSpeed * ParameterAccessor.Instance.RearDrill.SpeedToVelocityCount);
                  this.robotBody.SetDrillServoTravelVelocity(velocityCount);
                  this.drillRearIndexSpeedRequested = indexerSpeed;
               }

               this.robotBody.SetRearDrillIndex(this.drillRearIndexSetPoint);
               this.drillRearIndexRequested = this.drillRearIndexSetPoint;
            }

            if (this.drillFrontLaserSetPoint != this.drillFrontLaserRequested)
            {
               this.robotBody.SetFrontLaser(this.drillFrontLaserSetPoint);
               this.drillFrontLaserRequested = this.drillFrontLaserSetPoint;
            }

            if (this.drillRearLaserSetPoint != this.drillRearLaserRequested)
            {
               this.robotBody.SetRearLaser(this.drillRearLaserSetPoint);
               this.drillRearLaserRequested = this.drillRearLaserSetPoint;
            }
         }

         if (RobotApplications.inspect == ParameterAccessor.Instance.RobotApplication)
         {
         }
      }
Пример #2
0
      private void StartRobotBody()
      {
         this.robotBody.SetConsumerHeartbeat((UInt16)ParameterAccessor.Instance.RobotBus.ConsumerHeartbeatRate, (byte)ParameterAccessor.Instance.RobotBus.ControllerBusId);
         this.robotBody.SetProducerHeartbeat((UInt16)ParameterAccessor.Instance.RobotBus.ProducerHeartbeatRate);

         UlcRoboticsNicbotBody.Modes robotMode = UlcRoboticsNicbotBody.Modes.repair;

         if (RobotApplications.inspect == ParameterAccessor.Instance.RobotApplication)
         {
            robotMode = UlcRoboticsNicbotBody.Modes.inspect;
         }

         this.robotBody.Configure(robotMode);
         this.robotBody.Start();

         this.robotSolenoidSetPoint = this.robotBody.GetSolenoidCache();
         this.robotSolenoidRequested = this.robotSolenoidSetPoint;

         MovementWheelModes startWheelMode = MovementWheelModes.neither;
         bool lastAxial = this.robotBody.LastAxial;
         bool lastCircumferential = this.robotBody.LastCircumferential;

         if (false != lastAxial)
         {
            startWheelMode = MovementWheelModes.axial;
         }
         else if (false != lastCircumferential)
         {
            startWheelMode = MovementWheelModes.circumferential;
         }

         this.movementWheelModeChangeRequest = startWheelMode;
         this.movementWheelModeActual = startWheelMode;

         Thread.Sleep(50);

         if (RobotApplications.repair == ParameterAccessor.Instance.RobotApplication)
         {
            this.drillFrontConfigurationNeeded = true;
            this.drillRearConfigurationNeeded = true;
         }
      }
Пример #3
0
      public MovementWheelModes GetMovementWheelMode()
      {
         MovementWheelModes result = MovementWheelModes.neither;

         if (MovementWheelModes.neither != this.movementWheelModeActual)
         {
            result = this.movementWheelModeActual;
         }
         else if (MovementWheelModes.neither != this.movementWheelModeChangeRequest)
         {
            result = this.movementWheelModeChangeRequest;
         }
#if false
         MovementWheelModes startWheelMode = MovementWheelModes.neither;
         bool lastAxial = this.robotBody.LastAxial;
         bool lastCircumferential = this.robotBody.LastCircumferential;

         if (false != lastAxial)
         {
            startWheelMode = MovementWheelModes.axial;
         }
         else if (false != lastCircumferential)
         {
            startWheelMode = MovementWheelModes.circumferential;
         }

         this.movementWheelModeChangeRequest = startWheelMode;
         this.movementWheelModeActual = startWheelMode;

         this.movementWheelModeActual
         GetMovementWheelMode

         MovementWheelModes result = MovementWheelModes.neither;
         bool lastAxial = this.robotBody.LastAxial;
         bool lastCircumferential = this.robotBody.LastCircumferential;

         if (false != lastAxial)
         {
            result = MovementWheelModes.axial;
         }
         else if (false != lastCircumferential)
         {
            result = MovementWheelModes.circumferential;
         }
#endif
         return (result);
#if false
         bool wheelAxial = this.GetSolenoidActive(Solenoids.wheelAxial);
         bool wheelCircumferential = this.GetSolenoidActive(Solenoids.wheelCircumferential);
         MovementWheelModes result = MovementWheelModes.neither;

         if ((false == wheelAxial) && (false == wheelCircumferential))
         {
            result = MovementWheelModes.neither;
         }
         else if ((false == wheelAxial) && (false != wheelCircumferential))
         {
            result = MovementWheelModes.circumferential;
         }
         else if ((false != wheelAxial) && (false == wheelCircumferential))
         {
            result = MovementWheelModes.axial;
         }
         else
         {
            result = MovementWheelModes.both;
         }

         return (result);
#endif
      }
Пример #4
0
      public void SetMovementForwardMode(MovementForwardModes mode)
      {
         if (mode == MovementForwardModes.circumferential)
         {
            this.movementWheelModeChangeRequest = MovementWheelModes.circumferential;
         }
         else
         {
            this.movementWheelModeChangeRequest = MovementWheelModes.axial;
         }

         this.movementForwardMode = mode;
      }
Пример #5
0
      private void InitializeValues()
      {
         this.busReady = false;
         this.busStatus = null;
         this.busReceiveQueue.Clear();
         this.deviceResetQueue.Clear();

         this.robotBody.NodeId = (byte)ParameterAccessor.Instance.RobotBus.RobotBodyBusId;
         this.robotTopFrontWheel.NodeId = (byte)ParameterAccessor.Instance.RobotBus.RobotTopFrontWheelBusId;
         this.robotTopRearWheel.NodeId = (byte)ParameterAccessor.Instance.RobotBus.RobotTopRearWheelBusId;
         this.robotBottomFrontWheel.NodeId = (byte)ParameterAccessor.Instance.RobotBus.RobotBottomFrontWheelBusId;
         this.robotBottomRearWheel.NodeId = (byte)ParameterAccessor.Instance.RobotBus.RobotBottomRearWheelBusId;

         this.TraceMask = ParameterAccessor.Instance.RobotBus.ControllerTraceMask;
         this.robotBody.TraceMask = ParameterAccessor.Instance.RobotBus.RobotBodyTraceMask;
         this.robotTopFrontWheel.TraceMask = ParameterAccessor.Instance.RobotBus.RobotTopFrontWheelTraceMask;
         this.robotTopRearWheel.TraceMask = ParameterAccessor.Instance.RobotBus.RobotTopRearWheelTraceMask;
         this.robotBottomFrontWheel.TraceMask = ParameterAccessor.Instance.RobotBus.RobotBottomFrontWheelTraceMask;
         this.robotBottomRearWheel.TraceMask = ParameterAccessor.Instance.RobotBus.RobotBottomRearWheelTraceMask;

         this.InitializeRobotBody();

         this.movementMode = MovementModes.off;
         this.movementForwardMode = MovementForwardModes.normalAxial;
         this.movementWheelModeChangeRequest = MovementWheelModes.neither;
         this.movementWheelModeActual = MovementWheelModes.neither;
         this.movementTopFrontWheelStatus.Initialize();
         this.movementTopRearWheelStatus.Initialize();
         this.movementBottomFrontWheelStatus.Initialize();
         this.movementBottomRearWheelStatus.Initialize();

         this.drillFrontRotationSetPoint = 0;
         this.drillRearRotationSetPoint = 0;
         this.drillFrontIndexSetPoint = 0;
         this.drillRearIndexSetPoint = 0;
         this.drillFrontLaserSetPoint = false;
         this.drillRearLaserSetPoint = false;

         this.cameraA = CameraLocations.robotFrontUpperBack;
         this.cameraB = CameraLocations.robotLowerBack;
         this.videoASetPoint = 0;
         this.videoBSetPoint = 0;

         for (int i = 0; i < this.cameraLightLevelSetPoints.Length; i++)
         {
            this.cameraLightLevelSetPoints[i] = 0;
         }
      }