/// <summary>
        /// Clone ActuatorCoordination
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            ActuatorCoordination dmc = new ActuatorCoordination();

            dmc.RequestId = this.RequestId;
            dmc.Count     = this.Count;
            return(dmc);
        }
        /// <summary>
        /// CopyTo
        /// </summary>
        /// <param name="target"></param>
        public void CopyTo(IDssSerializable target)
        {
            ActuatorCoordination typedTarget = target as ActuatorCoordination;

            if (typedTarget == null)
            {
                throw new ArgumentException(string.Format("CopyTo({0}) requires type {0}", this.GetType().FullName));
            }
            typedTarget.Count     = this.Count;
            typedTarget.RequestId = this.RequestId;
        }
        public IEnumerator <ITask> SetDrivePowerHandler(SetDrivePower setDrivePower)
        {
            ValidateDriveConfiguration(false);
            _state.TimeStamp = DateTime.Now;

            PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>();

            // Add a coordination header to our motor requests
            // so that advanced motor implementations can
            // coordinate the individual motor reqests.
            coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(2);

            Motor.SetMotorPower leftPower = new Motor.SetMotorPower(setDrivePower.Body.LeftWheelPower);
            leftPower.ResponsePort = responsePort;
            leftPower.AddHeader(coordination);
            _leftMotorPort.Post(leftPower);

            Motor.SetMotorPower rightPower = new Motor.SetMotorPower(setDrivePower.Body.RightWheelPower);
            rightPower.ResponsePort = responsePort;
            rightPower.AddHeader(coordination);
            _rightMotorPort.Post(rightPower);

            // send notification to subscription manager
            drive.Update update = new drive.Update(_state);
            SendNotification <drive.Update>(_subMgrPort, update);

            Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
                                                                                    delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
            {
                if (successList.Count == 2)
                {
                    setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType());
                }

                foreach (Fault fault in failureList)
                {
                    setDrivePower.ResponsePort.Post(fault);
                    break;
                }
            }));

            yield break;
        }
Beispiel #4
0
        public IEnumerator<ITask> SetDrivePowerHandler(SetDrivePower setDrivePower)
        {
            ValidateDriveConfiguration(false);
            _state.TimeStamp = DateTime.Now;

            PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();

            // Add a coordination header to our motor requests
            // so that advanced motor implementations can
            // coordinate the individual motor reqests.
            coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(2);

            Motor.SetMotorPower leftPower = new Motor.SetMotorPower(setDrivePower.Body.LeftWheelPower);
            leftPower.ResponsePort = responsePort;
            leftPower.AddHeader(coordination);
            _leftMotorPort.Post(leftPower);

            Motor.SetMotorPower rightPower = new Motor.SetMotorPower(setDrivePower.Body.RightWheelPower);
            rightPower.ResponsePort = responsePort;
            rightPower.AddHeader(coordination);
            _rightMotorPort.Post(rightPower);

            // send notification to subscription manager
            drive.Update update = new drive.Update(_state);
            SendNotification<drive.Update>(_subMgrPort, update);

            Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
                delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
                {
                    if (successList.Count == 2)
                        setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType());

                    foreach (Fault fault in failureList)
                    {
                        setDrivePower.ResponsePort.Post(fault);
                        break;
                    }
                }));

            yield break;
        }
Beispiel #5
0
        public virtual IEnumerator <ITask> UpdateMotorSpeedHandler(UpdateMotorSpeed update)
        {
            //LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler()");

            coord.ActuatorCoordination coordination = update.GetHeader <coord.ActuatorCoordination>();
            if (coordination != null && coordination.Count > 1)
            {
                if (!_coordinationList.ContainsKey(coordination.RequestId))
                {
                    _coordinationList.Add(coordination.RequestId, new Port <UpdateMotorSpeed>());
                    Activate(
                        Arbiter.MultipleItemReceive <UpdateMotorSpeed>(
                            false,
                            _coordinationList[coordination.RequestId],
                            coordination.Count,
                            SetCoordinatedMotors));
                }
                _coordinationList[coordination.RequestId].Post(update);
                yield break;
            }

            bool changed = ((update.Body.LeftSpeed >= 0 && _state.MotorSpeed.LeftSpeed != update.Body.LeftSpeed) ||
                            (update.Body.RightSpeed >= 0 && _state.MotorSpeed.RightSpeed != update.Body.RightSpeed));

            if (update.Body.LeftSpeed != null)
            {
                if (update.Body.LeftSpeed >= -1.0 && update.Body.LeftSpeed <= 1.0)
                {
                    //LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - LeftSpeed=" + update.Body.LeftSpeed + " requested");
                    _state.MotorSpeed.LeftSpeed = update.Body.LeftSpeed;
                }
                else
                {
                    LogInfo("Error: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - invalid LeftSpeed=" + update.Body.LeftSpeed + " requested, must be between -1.0 and +1.0");
                }
            }

            if (update.Body.RightSpeed != null)
            {
                if (update.Body.RightSpeed >= -1.0 && update.Body.RightSpeed <= 1.0)
                {
                    //LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - RightSpeed=" + update.Body.RightSpeed + " requested");
                    _state.MotorSpeed.RightSpeed = update.Body.RightSpeed;
                }
                else
                {
                    LogInfo("Error: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - invalid RightSpeed=" + update.Body.RightSpeed + " requested, must be between -1.0 and +1.0");
                }
            }

            // If we are connected, send the speed to the robot wheels controller:
            if (ensureHardwareController())
            {
                double?leftSpeed  = _state.MotorSpeed.LeftSpeed;
                double?rightSpeed = _state.MotorSpeed.RightSpeed;

                Tracer.Trace("IP: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - speed  L: " + leftSpeed + "    R: " + rightSpeed);

                // it will take time for upper layers to react on whiskers. We want to have some protection here, to avoid damage.
                // Note: while moving forward the speeds are negative.
                // cannot move forward if whiskers are pressed; replace it with backwards movement at half speed though:
                if (leftSpeed < 0 && _state.Whiskers.FrontWhiskerLeft.GetValueOrDefault())
                {
                    Tracer.Trace("Warning: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - left whisker pressed, speed " + leftSpeed + " reversed");
                    leftSpeed = -leftSpeed / 2;
                }

                if (rightSpeed < 0 && _state.Whiskers.FrontWhiskerRight.GetValueOrDefault())
                {
                    Tracer.Trace("Warning: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - right whisker pressed, speed " + rightSpeed + " reversed");
                    rightSpeed = -rightSpeed / 2;
                }

                _brickConnection.SetSpeed(leftSpeed, rightSpeed);
            }

            // Send Notifications to subscribers
            if (changed)
            {
                _subMgrPort.Post(new submgr.Submit(_state.MotorSpeed, DsspActions.UpdateRequest));
            }

            _state.TimeStamp = _state.MotorSpeed.Timestamp = DateTime.Now;

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }