public override void SetMotors(float leftPower, float rightPower)
{
if (!motorsOn)
{
EnableMotors();
}
drive.SetDrivePowerRequest drivePowerReq = new drive.SetDrivePowerRequest();
drivePowerReq.LeftWheelPower = leftPower;
drivePowerReq.RightWheelPower = rightPower;
drive.SetDrivePower setDrivePower = new drive.SetDrivePower(drivePowerReq);
drivePort.Post(setDrivePower);
bool done = false;
Arbiter.Activate(DssEnvironment.TaskQueue,
Arbiter.Receive <DefaultUpdateResponseType>(false,
setDrivePower.ResponsePort,
delegate(DefaultUpdateResponseType state)
{
done = true;
}
));
while (!done)
{
;
}
}
public IEnumerator <ITask> AllStopHandler(drive.AllStop allStop)
{
drive.SetDrivePower zeroPower = new drive.SetDrivePower();
zeroPower.Body = new drive.SetDrivePowerRequest(0.0d, 0.0d);
zeroPower.ResponsePort = allStop.ResponsePort;
_mainPort.Post(zeroPower);
yield break;
}
public virtual IEnumerator <ITask> AllStopHandler(drive.AllStop update)
{
// Create a Drive request with both wheels stopped.
// Call the SetDrivePowerHandler, but redirect the responses
// to our response port.
drive.SetDrivePower setDrivePower = new drive.SetDrivePower(new drive.SetDrivePowerRequest(), update.ResponsePort);
SpawnIterator <drive.SetDrivePower>(setDrivePower, SetDrivePowerHandler);
yield break;
}
public IEnumerator <ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower)
{
if (setDrivePower.Body == null)
{
LogError("setDrivePower.Body == null");
setDrivePower.ResponsePort.Post(new Fault());
yield break;
}
double leftPower = setDrivePower.Body.LeftWheelPower;
double rightPower = setDrivePower.Body.RightWheelPower;
// Dump requests that come too fast.
// but let stop messages through
if (RequestPending > 1 && leftPower != 0 && rightPower != 0)
{
setDrivePower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
RequestPending++;
BoundsCheck(leftPower);
BoundsCheck(rightPower);
//update state
_state.TimeStamp = DateTime.Now;
_state.LeftWheel.MotorState.CurrentPower = leftPower;
_state.RightWheel.MotorState.CurrentPower = rightPower;
//convert to native units
int leftPowerScaled = (int)Math.Round(leftPower * 100 + 100);
int rightPowerScaled = (int)Math.Round(rightPower * 100 + 100);
//send hardware specific motor data
brick.SetMotorsBody motordata = new brick.SetMotorsBody(leftPowerScaled, rightPowerScaled);
Activate(Arbiter.Choice(_scribblerPort.SetMotors(motordata),
delegate(DefaultUpdateResponseType status)
{
setDrivePower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
RequestPending--;
},
delegate(soap.Fault failure)
{
setDrivePower.ResponsePort.Post(failure);
RequestPending--;
}
));
yield break;
}
public IEnumerator <ITask> SetDrivePowerHandler(drive.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();
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = setDrivePower.Body.LeftWheelPower
});
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = setDrivePower.Body.RightWheelPower
});
rightPower.ResponsePort = responsePort;
rightPower.AddHeader(coordination);
_rightMotorPort.Post(rightPower);
// send notification to subscription manager
Update update = new Update(_state);
SendNotification <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;
}
public IEnumerator <ITask> SetPowerHandler(diffdrive.SetDrivePower setPower)
{
if (_entity == null)
{
throw new InvalidOperationException("Simulation entity not registered with service");
}
if (!_state.IsEnabled)
{
setPower.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("SetPower request to disabled drive.");
yield break;
}
if ((setPower.Body.LeftWheelPower > 1.0f) || (setPower.Body.LeftWheelPower < -1.0f) ||
(setPower.Body.RightWheelPower > 1.0f) || (setPower.Body.RightWheelPower < -1.0f))
{
// invalid drive power
setPower.ResponsePort.Post(Fault.FromException(new Exception("Invalid Power parameter.")));
LogError("Invalid Power parameter in SetPowerHandler.");
yield break;
}
// Call simulation entity method for setting wheel torque
_entity.SetMotorTorque(
(float)(setPower.Body.LeftWheelPower),
(float)(setPower.Body.RightWheelPower));
UpdateStateFromSimulation();
setPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send update notification for entire state
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
yield break;
}
public virtual IEnumerator <ITask> SetDrivePowerHandler(drive.SetDrivePower update)
{
// For now, just dump requests that come in too fast.
if (_requestPending > 0)
{
update.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
//flip direction if necessary
double revPow = update.Body.LeftWheelPower;
if (_state.LeftWheel.MotorState.ReversePolarity)
{
revPow = (-1) * revPow;
}
//send left motor command
nxt.LegoSetOutputState motors = new nxt.LegoSetOutputState();
motors.OutputPort = _state.LeftWheel.MotorState.HardwareIdentifier - 1;
motors.PowerSetPoint = (int)(update.Body.LeftWheelPower * _state.LeftWheel.MotorState.PowerScalingFactor);
motors.Mode = nxt.LegoOutputMode.PowerControl;
motors.RegulationMode = nxt.LegoRegulationMode.MotorSpeed;
motors.TurnRatio = 0;
motors.RunState = nxt.LegoRunState.Running;
motors.RotationLimit = 0; //run forever
motors.RequireResponse = false;
_requestPending++;
PortSet <nxt.LegoResponse, Fault> resp1Port = _legoPort.SendLegoCommand(motors);
/* Activate(Arbiter.Choice(_legoPort.SendLegoCommand(motors),
* delegate(nxt.LegoResponse response)
* {
* _requestPending--;
* },
* delegate(Fault fault)
* {
* _requestPending--;
* }
* ));
*/
//send right motor command
motors = new nxt.LegoSetOutputState();
motors.OutputPort = _state.RightWheel.MotorState.HardwareIdentifier - 1;
motors.PowerSetPoint = (int)(update.Body.RightWheelPower * _state.RightWheel.MotorState.PowerScalingFactor);
motors.Mode = nxt.LegoOutputMode.PowerControl;
motors.RegulationMode = nxt.LegoRegulationMode.MotorSpeed;
motors.TurnRatio = 0;
motors.RunState = nxt.LegoRunState.Running;
motors.RotationLimit = 0; //run forever
motors.RequireResponse = false;
_requestPending++;
//Activate(Arbiter.Choice(_legoPort.SendLegoCommand(motors),
// delegate(nxt.LegoResponse response)
// {
// _requestPending--;
// },
// delegate(Fault fault)
// {
// _requestPending--;
// }
//));
yield return(Arbiter.Choice(_legoPort.SendLegoCommand(motors),
delegate(nxt.LegoResponse response)
{
_requestPending--;
},
delegate(Fault fault)
{
_requestPending--;
}
));
yield return(Arbiter.Choice(resp1Port,
delegate(nxt.LegoResponse response)
{
_requestPending--;
},
delegate(Fault fault)
{
_requestPending--;
}
));
LogInfo(update.Body.LeftWheelPower * 100 + ":" + update.Body.RightWheelPower * 100);
_state.TimeStamp = DateTime.Now;
// forward notification
SendNotification(_subMgrPort, new drive.Update(_state));
//reply to sender
update.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
public IEnumerator<ITask> AllStopHandler(drive.AllStop allStop)
{
/*
from http://social.msdn.microsoft.com/Forums/en-US/roboticssimulation/thread/13fb26c8-75fe-43b9-9e65-6a915e7d2560 :
In RDS 2008 there is some discussion of the Generic Differential Drive in the Help file.
AllStop should cause the current operation to be cancelled, as you expect.
It will then disable the drive and you will have to issue an EnableDrive request before you can get the robot to move again.
This is by design so that AllStop can act as an Emergency Stop and prevent any further action without an explicit reset.
There should be a notification from the drive service that the drive power has been set to zero after the AllStop.
*/
#if TRACELOG
LogInfo("TrackRoamerDriveService:: AllStopHandler()");
#endif
cancelCurrentOperation();
drive.SetDrivePower zeroPower = new drive.SetDrivePower();
zeroPower.Body = new drive.SetDrivePowerRequest(0.0d, 0.0d);
zeroPower.ResponsePort = allStop.ResponsePort;
_mainPort.Post(zeroPower);
pollEncoderState(); // get starting encoder state
yield break;
}
public IEnumerator<ITask> AllStopHandler(drive.AllStop allStop)
{
drive.SetDrivePower zeroPower = new drive.SetDrivePower();
zeroPower.Body = new drive.SetDrivePowerRequest(0.0d, 0.0d);
zeroPower.ResponsePort = allStop.ResponsePort;
_mainPort.Post(zeroPower);
yield break;
}
