public void Here(IK robot, Transform target)
{
if (gameController.GetOnlineMode())
{
// 2 options. use pos from robot (simulation) or real robot
if (isHereFromSimulation)
{
// TEST????
// Copy angles from robot
target.position = new Vector3(robot.GetE().position.x, robot.GetE().position.y, robot.GetE().position.z);
target.GetComponent <TargetModel>().SetAngles(robot.GetAngles());
// Get pos and p, r from simulation. Do teach to real Scorbot
List <float> xyzpr = new List <float>()
{
target.position.x, target.position.z, target.position.y,
target.GetComponent <TargetModel>().GetPitch(), -target.GetComponent <TargetModel>().GetRoll()
};
controller.Connection.GetComponent <SerialController>().WriteToControllerTeach(target.GetComponent <TargetModel>().name, xyzpr);
//Teach(robot, target, pos, posPitchRoll[3], posPitchRoll[4]);
stateOutput.text = "Success Here";
}
else // From real Scorbot
{
List <String[]> listString = new List <string[]>();
// This stops main thread
//controller.RunCommandHereOnline(target.GetComponent<TargetModel>().GetName());
//listString = controller.RunCommandListpvOnline(target.GetComponent<TargetModel>().GetName());
string result = "";
Regex rx = new Regex("^.:(.+?)$");
List <int> counts = new List <int>();
List <float> posPitchRoll = new List <float>();
foreach (String[] a in listString)
{
//aux += a.Length.ToString();
foreach (string b in a)
{
//Debug.Log(b);
MatchCollection matches = rx.Matches(b);
foreach (Match match in matches)
{
GroupCollection groups = match.Groups;
result += groups[1].Value + "?";
posPitchRoll.Add(float.Parse(groups[1].Value));
}
}
}
for (int i = 0; i < 5; i++)
{
counts.Add((int)posPitchRoll[0]);
posPitchRoll.RemoveAt(0);
}
result += posPitchRoll.Count + " " + counts.Count + " " + counts[0] + " " + posPitchRoll[0];
stateOutput.text = result;
// Just set robot to angles of real robot, then teach? with POS PITCH ROLL recovered (use target)
//target.GetComponent<TargetModel>().SetAngles(robot.transform.GetComponent<ScorbotModel>().CountsToAngles(counts));
//target.position = robot.GetPosFromCounts(counts);
// 18 19.8 46.9
// Or do teach to get same pos
Teach(robot, target, new Vector3(posPitchRoll[0], posPitchRoll[1], posPitchRoll[2]), posPitchRoll[3], posPitchRoll[4], false);
}
}
else // Offline
{
// Copy angles from robot
target.position = new Vector3(robot.GetE().position.x, robot.GetE().position.y, robot.GetE().position.z);
target.GetComponent <TargetModel>().SetAngles(robot.GetAngles());
//List<int> counts = new List<int>() { -13541, -22691, -3489, 56937, 27};
//target.position = robot.GetPosFromCounts(counts);
//stateOutput.text = robot.GetPosFromCounts(counts).ToString();
}
}
/**
* Modifica una posición con los valores actuales del Scorbot. Modifica una posición en el controlador con los
* valores actuales del Scorbot real o el de la simulación. En modo "From simulation" el Scorbot es el de
* la simulación, mientras que en modo "From Scorbot" es el Scorbot real. En modo "From simulation" se ejecuta
* el comando "Teach" (online) con los valores del Scorbot de la simulación. En modo "From Scorbot" se ejecuta
* el comando "Here" (online) en el Scorbot real, seguidamente de "Listpv" (online) para recuperar los datos
* del "Here" y se realiza un "Teach" (Offline) para cargar esos datos.
* @param robot Scorbot
* @param target Posición (objeto)
* @return void
*/
public void Here(IK robot, Transform target)
{
// Online mode
if (gameController.GetOnlineMode())
{
// 2 options. Use pos from simulation or real Scorbot
if (isHereFromSimulation) // Here. Mode "From simulation"
{
// Copy angles from simulation into the position (object)
target.position = new Vector3(robot.GetE().position.x, robot.GetE().position.y, robot.GetE().position.z);
target.GetComponent <TargetModel>().SetAngles(robot.GetAngles());
stateMessageControl.WriteMessage("Done. HERE \"" + target.GetComponent <TargetModel>().GetName() + "\"", true);
target.GetComponent <TargetModel>().SetSync(false);
stateMessageControl.UpdatePositionLog();
// Get pos, p, r from simulation. Do teach to real Scorbot
float multPos = 10f;
float multDegrees = 1f;
if (robot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERVPlus.INDEX)
{
multPos = 100f;
multDegrees = 10f;
}
List <float> xyzpr = new List <float>()
{
target.position.x *multPos, target.position.z *multPos, target.position.y *multPos,
target.GetComponent <TargetModel>().GetPitch() * multDegrees, target.GetComponent <TargetModel>().GetRoll() * multDegrees
};
bool done = controller.RunCommandUITeach(target.GetComponent <TargetModel>().GetName(), xyzpr);
if (done)
{
stateMessageControl.WriteMessage("Done. Online HERE \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
target.GetComponent <TargetModel>().SetSync(true);
stateMessageControl.UpdatePositionLog();
}
else
{
stateMessageControl.WriteMessage("Error. Online HERE \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
}
}
else // Here. Mode "From real Scorbot"
{
// Real scorbot here
bool here = controller.RunCommandUIOnline("here", target.GetComponent <TargetModel>().GetName());
if (here)
{
stateMessageControl.WriteMessage("Done. Online HERE(HERE) \"" + target.GetComponent <TargetModel>().GetName() + "\"", here);
target.GetComponent <TargetModel>().SetSync(false);
stateMessageControl.UpdatePositionLog();
}
else
{
stateMessageControl.WriteMessage("Error. Online HERE(HERE) \"" + target.GetComponent <TargetModel>().GetName() + "\"", here);
}
// Get data from real Scorbot into the position (object)
Thread.Sleep(200);
bool done = SyncScorbotToSimulation(robot, target);
if (done)
{
stateMessageControl.WriteMessage("Done. Online HERE(SYNC) \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
}
else
{
stateMessageControl.WriteMessage("Error. Online HERE(SYNC) \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
}
}
}
else // Offline mode
{
// Copy angles from simulation into the position (object)
target.position = new Vector3(robot.GetE().position.x, robot.GetE().position.y, robot.GetE().position.z);
target.GetComponent <TargetModel>().SetAngles(robot.GetAngles());
target.GetComponent <TargetModel>().SetSync(false);
stateMessageControl.UpdatePositionLog();
stateMessageControl.WriteMessage("Done. HERE \"" + target.GetComponent <TargetModel>().GetName() + "\"", true);
}
}
