IEnumerator TargetMove(Vector3 targetPos, List <List <float> > midsTargetAngles)
{
var startAngle = robot.GetAngles();
var deltas = new List <float>();
deltas.Add(GetMagnitude(startAngle, midsTargetAngles[0]));
for (var i = 1; i < midsTargetAngles.Count(); ++i)
{
deltas.Add(GetMagnitude(midsTargetAngles[i - 1], midsTargetAngles[i]));
}
var allDelta = deltas.Sum();
var phasies = deltas.Select(d => d / allDelta).ToList();
var moveTime = allDelta / aveSpeed;
var currentTime = 0.0f;
int moveloop = 0;
print("moving");
int index = 0;
FKManager.Foreach(linkParams, midsTargetAngles[index], fk);
float offset = 0;
float progress = 0;
while (!IsCloseEnough(robot.CurrentEndPosition(), targetPos) && moveloop < maxMoveLoop && currentTime < moveTime)
{
if (progress >= 1)
{
offset += phasies[index];
++index;
startAngle = robot.GetAngles();
FKManager.Foreach(linkParams, midsTargetAngles[index], fk);
}
robot.SetAngle(AnglesDeltaByPhase(robot.GetAngles(), startAngle, midsTargetAngles[index], offset, phasies[index], out progress, currentTime, moveTime));
moveloop++;
currentTime += Time.deltaTime;
yield return(null);
}
}
IEnumerator AngleMove(List <float> targetAngle)
{
int moveloop = 0;
var moveTime = GetMagnitude(robot.GetAngles(), targetAngle) / aveSpeed;
var currentTime = 0.0f;
FKManager.Foreach(linkParams, targetAngle, fk);
print("moving");
var startAngle = robot.GetAngles();
while (robot.GetAngles().Where(a => a != 0).Any() && moveloop < maxMoveLoop && currentTime < moveTime)
{
robot.SetAngle(AnglesDelta(robot.GetAngles(), startAngle, targetAngle, currentTime, moveTime));
moveloop++;
currentTime += Time.deltaTime;
yield return(null);
}
}
IEnumerator TargetMove(Vector3 targetPos, List <float> targetAngle)
{
var startAngle = robot.GetAngles();
var moveTime = GetMagnitude(robot.GetAngles(), targetAngle) / aveSpeed;
var currentTime = 0.0f;
int moveloop = 0;
FKManager.Foreach(linkParams, targetAngle, fk);
print("moving");
while (!IsCloseEnough(robot.CurrentEndPosition(), targetPos) && moveloop < maxMoveLoop && currentTime < moveTime)
{
robot.SetAngle(AnglesDelta(robot.GetAngles(), startAngle, targetAngle, currentTime, moveTime));
moveloop++;
currentTime += Time.deltaTime;
yield return(null);
}
}
// Update is called once per frame
void FixedUpdate()
{
if (!isInControl)
{
if (!isIK)
{
if (ikCoroutine != null)
{
StopCoroutine(ikCoroutine);
ikCoroutine = null;
}
//print($"angles { thetas[0] * Mathf.Rad2Deg}, { thetas[1] * Mathf.Rad2Deg}, { thetas[2] * Mathf.Rad2Deg}, { thetas[3] * Mathf.Rad2Deg}, { thetas[4] * Mathf.Rad2Deg}, { thetas[5] * Mathf.Rad2Deg}");
currentAngles = new List <float>()
{
angle1 *Mathf.Deg2Rad, angle2 *Mathf.Deg2Rad, angle3 *Mathf.Deg2Rad, angle4 *Mathf.Deg2Rad, angle5 *Mathf.Deg2Rad, angle6 *Mathf.Deg2Rad
};
SetAngles(currentAngles);
var readThetas = new List <float>();
foreach (var joint in joints)
{
readThetas.Add(joint.GetPosition());
}
HTMs = hC.GetHTMs(readThetas);
EndHTM = HTMs[HTMs.Count() - 1];
fk.Foreach(HTMs);
}
else
{
if (ikCoroutine == null)
{
ikCoroutine = StartCoroutine(IKAuto());
}
SetAngles(currentAngles);
}
}
}
public static void Foreach(List <LinkParam> linkParams, List <float> angles, FKManager fk)
{
fk.Foreach(HomogeneourCoordinate.GetHTMs(linkParams, angles));
}
