// ---------------------------------- All controls ------------------------------
void Update()
{
// Menu activation
if (Input.GetKeyDown(KeyCode.Escape))
{
menu.SetActive(true);
}
// Robot values
if (!robot)
{
return;
}
output.text = "Angles: \n";
for (int artIndex = robot.GetArticulations().Length - 1; artIndex >= 0; artIndex--)
{
output.text += robot.GetArticulations()[artIndex].Angle() + "\n";
}
output.text += "Efector pos: \n" + robot.GetE().position + "\n";
//robot.articulations[3].UpdateAngleAsGlobal(new Vector3(0f, 0f, 45f));
/*
* output.text = "Efector pos: \n" + robot.GetE().position + "\n";
*
* output.text += robot.GetArticulations()[3].Angle() + "\n";
* output.text += robot.GetArticulations()[3].transform.rotation.eulerAngles + "\n";
*/
}
void Update()
{
// Activate/deactivate keyboard control
if (!processing)
{
return;
}
if (!robot)
{
return;
}
// Scorbot articulation rotation (See keyCodes at "Start()")
int j = 0;
for (int i = 0; i < robot.GetArticulations().Length; i++)
{
if (Input.GetKey(keyCodes[j]))
{
robot.GetArticulations()[i].Rotate(-ROTATION_SENSIBILITY);
}
j++;
if (Input.GetKey(keyCodes[j]))
{
robot.GetArticulations()[i].Rotate(ROTATION_SENSIBILITY);
}
j++;
}
// Open/close Scorbot end efector
if (Input.GetKey(KeyCode.Y))
{
robot.GetComponent <ScorbotModel>().Open();
}
if (Input.GetKey(KeyCode.Alpha6))
{
robot.GetComponent <ScorbotModel>().Close();
}
}
// mm. pos in real Scorbot
public void Teach(IK robot, Transform target, Vector3 pos, float p, float r, bool online = true)
{
Vector3 posReal = new Vector3(pos.x, pos.y, pos.z);
// mm to cm. pos in simulation
pos = new Vector3(pos.x / 10f, pos.z / 10f, pos.y / 10f);
Vector3 startPos = target.position;
Vector3 startPitch = target.GetComponent <TargetModel>().GetAngles()[3];
Vector3 startRoll = target.GetComponent <TargetModel>().GetAngles()[4];
if (target.GetComponent <TargetModel>().GetAngles() == null)
{
stateOutput.text = "No angles data";
//Debug.Log("No angles data");
//return;
// Just fill something
List <Vector3> angles = new List <Vector3>(5);
for (int i = 0; i < 5; i++)
{
angles.Add(Vector3.zero);
}
target.GetComponent <TargetModel>().SetAngles(angles);
//Debug.Log(angles.Count);
}
// DO HERE and recover angles?
// Apply pitch and roll to target
target.GetComponent <TargetModel>().GetAngles()[3] = robot.GetArticulations()[3].BuiltAngle(p);
target.GetComponent <TargetModel>().GetAngles()[4] = robot.GetArticulations()[4].BuiltAngle(-r);
// Check if it's an unreachable point
target.position = pos;
if (!robot.TargetInRange(target, true))
{
//stateOutput.text = "Unreachable point";
//Debug.Log("Unreachable Teach");
stateOutput.text = "Unreachable Teach";
// Restore target
target.position = startPos;
target.GetComponent <TargetModel>().GetAngles()[3] = startPitch;
target.GetComponent <TargetModel>().GetAngles()[4] = startRoll;
return;
}
target.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
//Debug.Log("Success Teach");
stateOutput.text = "Success Teach";
// Calculate new config with new restrictions
if (gameController.GetOnlineMode() && online)
{
List <float> xyzpr = new List <float>()
{
posReal.x, posReal.y, posReal.z, p, r
};
controller.Connection.GetComponent <SerialController>().WriteToControllerTeach(target.GetComponent <TargetModel>().GetName(), xyzpr);
stateOutput.text = "Success Online Teach";
}
}
/**
* Modifica una posición con nuevos valores los cuales son una posición (x, y, z), inclinación frontal e
* inclinación lateral. Estos valores deben entar en el contexto del Scorbot real.
* @param robot Scorbot
* @param target Posición (objeto)
* @param pos Coordenadas
* @param p Pitch
* @param r Roll
* @param online Ejecutar modo online
* @param offline Ejecutar modo offline
* @return bool Éxito
*/
public bool Teach(IK robot, Transform target, Vector3 pos, float p, float r, bool online = true, bool offline = true)
{
Vector3 posReal = new Vector3(pos.x, pos.y, pos.z);
// Offline mode
if (offline)
{
// mm to cm. Interchange y and z. Position in simulation
pos = new Vector3(pos.x / 10f, pos.z / 10f, pos.y / 10f);
// Copy initial values
Vector3 startPos = target.position;
Vector3 startPitch = target.GetComponent <TargetModel>().GetAngles()[3];
Vector3 startRoll = target.GetComponent <TargetModel>().GetAngles()[4];
// Apply pitch and roll to target
target.GetComponent <TargetModel>().GetAngles()[3] = robot.GetArticulations()[3].BuiltAngle(p);
float auxR = -r;
if (robot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERVPlus.INDEX)
{
auxR = r;
}
target.GetComponent <TargetModel>().GetAngles()[4] = robot.GetArticulations()[4].BuiltAngle(auxR);
// Apply new coordinatesC
target.position = pos;
// Check if it's an unreachable point
if (!robot.TargetInRange(target, true))
{
stateMessageControl.WriteMessage("Error. TEACH Unreachable position \"" + target.GetComponent <TargetModel>().GetName() + "\"", false);
// Restore position (object) values
target.position = startPos;
target.GetComponent <TargetModel>().GetAngles()[3] = startPitch;
target.GetComponent <TargetModel>().GetAngles()[4] = startRoll;
return(false);
}
// Recover angles data. Apply to position (object)
target.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
target.GetComponent <TargetModel>().SetSync(false);
stateMessageControl.WriteMessage("Done. TEACH \"" + target.GetComponent <TargetModel>().GetName() + "\"", true);
stateMessageControl.UpdatePositionLog();
// if this position is being used by another position (relative), that position is not sync anymore
Transform relativePosition = target.GetComponent <TargetModel>().GetRelativeFrom();
if (target.GetComponent <TargetModel>().GetRelativeFrom())
{
relativePosition.GetComponent <TargetModel>().SetSync(false);
// Updating relative position
relativePosition.GetComponent <TargetModel>().UpdateRelativePosition();
// Update angles data
if (robot.TargetInRange(relativePosition))
{
// Reachable. Load data to target
relativePosition.GetComponent <TargetModel>().SetAngles(robot.GetAnglesFromCopy());
relativePosition.GetComponent <TargetModel>().SetValid(true);
}
else // Unreachable
{
relativePosition.GetComponent <TargetModel>().SetValid(false);
}
}
// This position is relative to another, teach destroys relativity
if (target.GetComponent <TargetModel>().GetRelativeTo())
{
target.GetComponent <TargetModel>().SetNoRelativeTo();
}
}
// Online mode
if (gameController.GetOnlineMode() && online)
{
// Build data to send
if (robot.GetComponent <ScorbotModel>().scorbotIndex == ScorbotERVPlus.INDEX)
{
posReal = posReal * 10f;
p = p * 10f;
r = r * 10f;
}
List <float> xyzpr = new List <float>()
{
posReal.x, posReal.y, posReal.z, p, r
};
bool done = controller.RunCommandUITeach(target.GetComponent <TargetModel>().GetName(), xyzpr);
if (done)
{
stateMessageControl.WriteMessage("Done. Online TEACH \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
target.GetComponent <TargetModel>().SetSync(true);
if (target.GetComponent <TargetModel>().GetRelativeFrom())
{
target.GetComponent <TargetModel>().GetRelativeFrom().GetComponent <TargetModel>().SetSync(true);
}
stateMessageControl.UpdatePositionLog();
}
else
{
stateMessageControl.WriteMessage("Error. Online TEACH \"" + target.GetComponent <TargetModel>().GetName() + "\"", done);
return(false);
}
}
return(true);
}
