public override void DoActions(PhysicalRobotAction action)
{
//Console.WriteLine("Sending action to simulator");
Random rng = new Random();
if (action.ActionType == PhysicalActionType.MoveForward)
{
//Accounts for scaling differences in the simulator and the controller
action.ActionValue = (action.ActionValue / 1000) * this.scale;
Console.WriteLine("=====================MOVING FORWARD THIS DISTANCE: " + action.ActionValue);
CS266.SimCon.Simulator.RobotActions rb = new RobotActions();
rb.Add(new RobotAction(action.RobotId.ToString(), 0, (float)action.ActionValue));
os.ExecuteActions(rb);
}
else if (action.ActionType == PhysicalActionType.MoveBackward)
{
//Accounts for scaling differences in the simulator and controller
action.ActionValue = (action.ActionValue / 1000) * this.scale;
CS266.SimCon.Simulator.RobotActions rb = new RobotActions();
rb.Add(new RobotAction(action.RobotId.ToString(), 0, (float)-action.ActionValue));
os.ExecuteActions(rb);
}
else if (action.ActionType == PhysicalActionType.Turn)
{
if (os.Algo.name != "DFS")
{
if (action.ActionValue < 0)
{
action.ActionValue = (-1) * (360f + action.ActionValue);
}
else
{
action.ActionValue *= -1;
}
}
Console.WriteLine("=====================TURNING THIS ANGLE: " + action.ActionValue);
CS266.SimCon.Simulator.RobotActions rb = new RobotActions();
//rb.Add(new RobotAction(action.RobotId.ToString(), (float)action.ActionValue, 0));
rb.Add(new RobotAction(action.RobotId.ToString(), (-1) * (float)action.ActionValue, 0));
// multiply by -1 to undo what Robot.cs does!
os.ExecuteActions(rb);
}
else if (action.ActionType == PhysicalActionType.CreateRobot)
{
os.AddNewRobot(new ObjectState("N1", "robot", new float[3] { DFSExperiment.doorX, DFSExperiment.doorY, 0 }, new float[3] { 1, 0, 0 }, new float[3] { 0, 0, 0 }, new float[3] { 0, 0, 0 }));
}
else if (action.ActionType == PhysicalActionType.SetSpeed) { }
//System.Threading.Thread.Sleep(500);
}
public WorldState RobotsAct(RobotActions actions_vector)
{
//Iterate over the robots and the actions
for (int i = 0; i < this.RobotList.Count; i++)
{
for (int j = 0; j < actions_vector.size; j++)
{
//Console.WriteLine("A******" + this.RobotList[i].State.Name);
//Console.WriteLine("B******" + actions_vector.actions[j].name);
//If the name of the robot and the particular action matchup
if (this.RobotList[i].State.Name == actions_vector.actions[j].name)
{
if (actions_vector.actions[j].newdegreesfromx > .5)
{
//Console.WriteLine("ROBOT TURN ANGLE IS EQUAL TO: " + actions_vector.actions[j].newdegreesfromx);
//Rotate by the stated number of degrees
//Speed was .1f. . . current value seems reasonable, but .1f is always okay
this.RobotList[i].RotateDegrees(actions_vector.actions[j].newdegreesfromx, .5f);
Thread.Sleep(2500);
}
if (actions_vector.actions[j].distance > .1)
{
//Drive the correct distance
//Console.WriteLine("ROBOT DRIVE DISTANCE IS EQUAL TO: " + actions_vector.actions[j].distance);
this.RobotList[i].DriveDistance(actions_vector.actions[j].distance, 1f);
Thread.Sleep(2000);
break;
}
}
}
}
//The Sleep function may be an issue!!!!!!!
//Thread.Sleep(10000);
//WorldStateParser WP = new WorldStateParser("SimulationEngineState.xml");
//WorldStateParser WP = new WorldStateParser("http://localhost:50000/simulationengine");
//WorldState WS = WP.parse();
// We just need an empty world state
return new WorldState(new List<CS266.SimCon.Simulator.ObjectState>());
}
//NEW!!!!! Changed!
public void ExecuteActions(RobotActions ActionsVector)
{
//Console.WriteLine("SEND RECEIVED ACTION");
this.W = this.RobotsAct(ActionsVector);
}
