/**
** Sequence parsing logic, use executeSequence() to run a sequence file
**/
IEnumerator parseSequence(string filecontent, bool validate, int repeat, float speedModifier)
{
bool hasHeader = false;
int lineNr = 0;
string[] lines = filecontent.Split('\n');
Dictionary <int, SpiderLeg.LegMovement> tmpFrame = null;
int tmpFrameMillis = -1;
for (int i = 0; i < repeat; i++)
{
foreach (string line in lines)
{
lineNr += 1;
if (line.TrimStart(' ').StartsWith("#"))
{
continue;
}
string cleanline = line.Replace("\r", string.Empty);
string[] words = cleanline.Split(' ');
string command = words[0];
if (!hasHeader && !words[0].ToLower().Equals("sequence"))
{
Debug.Log("Sequencefile has an invalid header, it should start with 'Sequence <sequencename>'");
yield break;
}
else if (lineNr == 1)
{
hasHeader = true;
if (words.Length > 3)
{
this.offset = int.Parse(words[3]);
}
else
{
this.offset = 0;
}
continue;
}
if (speedModifier < 0)
{
if (command == "framebegin")
{
command = "frameend";
}
else if (command == "frameend")
{
command = "framebegin";
}
}
if (words[0].ToLower().Equals("framebegin"))
{
if (words.Length > 1)
{
tmpFrameMillis = int.Parse(words[1]);
}
tmpFrame = new Dictionary <int, SpiderLeg.LegMovement>();
}
else
if (words[0].ToLower().Equals("frameend"))
{
if (tmpFrame == null)
{
continue;
}
SpiderLeg.SequenceFrame sf = SpiderLeg.newSequenceFrame(tmpFrame, tmpFrameMillis);
tmpFrameMillis = -1;
for (int x = 1; x < 7; x++)
{
SpiderLeg.LegMovement mov;
bool hasMov = sf.movements.TryGetValue(x, out mov);
if (!hasMov)
{
mov = new SpiderLeg.LegMovement();
mov.empty = true;
}
mov.maxExecTime = sf.maxMaxExecTime;
legs[x - 1].addMove(mov);
}
tmpFrame.Clear();
tmpFrame = null;
}
else
if (words[0].ToLower().Equals("delay"))
{
if (words.Length != 2)
{
Debug.Log("No argument given for delay at line: " + lineNr);
yield break;
}
float seconds = int.Parse(words[1]) / 1000f;
if (!validate)
{
Debug.Log("Will delay " + seconds + " seconds");
yield return(new WaitForSeconds(seconds * delayModifier));
}
}
else
if (words[0].ToLower().StartsWith("waitlegs"))
{
//bool shouldBlock = true;
bool allEmpty = false;
while (!allEmpty)
{
float maxExec = 0f;
foreach (SpiderLeg leg in legs)
{
allEmpty = true;
maxExec = Mathf.Max(maxExec, leg.maxExecTime);
if (leg.hasMovesOnQueue())
{
allEmpty = false;
yield return(new WaitForSeconds(maxExec));
}
}
}
}
if (words[0].ToLower().StartsWith("s:"))
{
if (words.Length < 2 || words.Length > 3)
{
Debug.Log("Wrong amount of arguments for servo control: " + words.Length + " at line: " + lineNr);
yield break;
}
int servoID = int.Parse(words[0].Split(':')[1]);
string[] coords = words[1].Split(',');
int speed = -1;
if (words.Length == 3)
{
speed = int.Parse(words[2]);
}
speed = (int)(speed * speedModifier);
if (!validate)
{
float newAngle = float.Parse(coords[0]);
//Debug.Log("Will control servo " + servoID + ", coords: " + newAngle + ", speed: " + speed);
foreach (SpiderLeg leg in legs)
{
leg.moveByServoID(servoID, newAngle, speed, false);
}
//#hier de servocontroller aanroepen met de variabelen wanneer not validate
}
}
else
//Controll legs
if (words[0].ToLower().StartsWith("l:"))
{
if (words.Length < 2 || words.Length > 3)
{
Debug.Log("Wrong amount of arguments for servo control: " + words.Length + " at line: " + lineNr);
yield break;
}
int legID = int.Parse(words[0].Split(':')[1]);
string[] coords = words[1].Split(",".ToCharArray());
if (coords.Length != 3)
{
Debug.Log("Wrong amount of coords: " + coords.Length + " at line: " + lineNr);
yield break;
}
int speed = -1;
if (words.Length == 3)
{
speed = (int)(int.Parse(words[2]));
}
speed = (int)(speed * speedModifier);
if (!validate)
{
//Debug.Log("Will control leg " + legID + ", coords: " + words[1] + ", speed: " + speed);
SpiderLeg.LegMovement lm = setServoPos(float.Parse(coords[0]), float.Parse(coords[1]), float.Parse(coords[2]), legID, speed);
if (tmpFrame == null)
{
this.legs[legID - 1].addMove(lm);
}
else
{
tmpFrame.Add(legID, lm);
}
}
}
}
}
}
private SpiderLeg.LegMovement setServoPos(float x, float y, float z, int legID, float speed)
{
if (legID == 1 || legID == 6 || legID == 5)
{
y *= -1;
}
float lIK = (float)Math.Sqrt(Math.Pow((d + lc + x), 2) + (Math.Pow(y, 2)));
float dIK = lIK - lc;
float bIK = (float)Math.Sqrt(Math.Pow((e + z), 2) + Math.Pow(dIK, 2));
//# determine current position of servos
float coxaCurr = servoAngleMap[((legID - 1) * 3)]; //self.servoController.getPosition(coxaServoId)
float femurCurr = servoAngleMap[((legID - 1) * 3) + 1]; //self.servoController.getPosition(femurServoId)
float tibiaCurr = servoAngleMap[((legID - 1) * 3) + 2];; //self.servoController.getPosition(tibiaServoId)
//Debug.Log(("current positions: " + coxaCurr + ", " + femurCurr + ", " + tibiaCurr));
//float alphaIK = (float) Math.Acos((Math.Pow(bIK, 2) + Math.Pow(c, 2) - Math.Pow(a, 2)) / (2 * bIK * c));
float betaIK = (float)Math.Acos((Math.Pow(a, 2) + Math.Pow(c, 2) - Math.Pow(bIK, 2)) / (2 * a * c));
float gammaIK = (float)Math.Acos((Math.Pow(a, 2) + Math.Pow(bIK, 2) - Math.Pow(c, 2)) / (2 * a * bIK));
float thetaIK = (float)Math.Asin(y / lIK);
float tauIK = (float)Math.Atan((e + z) / dIK);
float angleCoxa = thetaIK * (180 / Mathf.PI);
float angleFemur = -((gammaIK - tauIK) * (180 / Mathf.PI));
float angleTibia = 180 - ((betaIK) * (180 / Mathf.PI));
if (legID == 1 || legID == 4)
{
angleCoxa -= offset;
}
else if (legID == 5 || legID == 2)
{
angleCoxa += offset;
}
float deltaCoxa = Quaternion.Angle(Quaternion.Euler(0, angleCoxa, 0), Quaternion.Euler(0, coxaCurr, 0));
float deltaFemur = Quaternion.Angle(Quaternion.Euler(0, 0, angleFemur), Quaternion.Euler(0, 0, femurCurr));; //Math.Abs(angleFemur - femurCurr);
float deltaTibia = Quaternion.Angle(Quaternion.Euler(0, 0, angleTibia), Quaternion.Euler(0, 0, tibiaCurr)); //Math.Abs(angleTibia - tibiaCurr);
Debug.Log(("deltas " + deltaCoxa + ", " + deltaFemur + ", " + deltaTibia));
servoAngleMap[(legID - 1) * 3] = angleCoxa;
servoAngleMap[(legID - 1) * 3 + 1] = angleFemur;
servoAngleMap[(legID - 1) * 3 + 2] = angleTibia;
float maxDelta = 0;
maxDelta = Math.Max(deltaCoxa, deltaFemur);
maxDelta = Math.Max(maxDelta, deltaTibia);
if (maxDelta == 0)
{
//Debug.LogError("0 Delta moving to: " + x + "," + y + "," + z + " LEG: " + legID);
}
float coxaSpeed = 0, femurSpeed = 0, tibiaSpeed = 0;
if (maxDelta != 0)
{
if (maxDelta == deltaCoxa)
{
//Debug.Log(("max delta is coxa"));
coxaSpeed = speed;
femurSpeed = (int)(Math.Round(speed * (deltaFemur / maxDelta), 0));
tibiaSpeed = (int)(Math.Round(speed * (deltaTibia / maxDelta), 0));
}
else if (maxDelta == deltaFemur)
{
//Debug.Log(("max delta is femur"));
coxaSpeed = (int)(Math.Round(speed * (deltaCoxa / maxDelta), 0));
femurSpeed = speed;
tibiaSpeed = (int)(Math.Round(speed * (deltaTibia / maxDelta), 0));
}
else if (maxDelta == deltaTibia)
{
//Debug.Log(("max delta is tibia"));
coxaSpeed = (int)(Math.Round(speed * (deltaCoxa / maxDelta), 0));
femurSpeed = (int)(Math.Round(speed * (deltaFemur / maxDelta), 0));
tibiaSpeed = speed;
}
}
//Debug.Log(("angleCoxa: " + angleCoxa + " speed: " + coxaSpeed));
//Debug.Log(("angleFemur: " + angleFemur + " speed: " + femurSpeed));
//Debug.Log(("angleTibia: " + angleTibia + " speed: " + tibiaSpeed));
SpiderLeg.LegMovement lm = new SpiderLeg.LegMovement();
lm.coxa = angleCoxa;
lm.coxaSpeed = coxaSpeed;
lm.femur = angleFemur;
lm.femurSpeed = femurSpeed;
lm.tibia = angleTibia;
lm.tibiaSpeed = tibiaSpeed;
float timePerAngle = (float)(100 * 360.0 / 60.0) * (speed / 1023f);
lm.maxExecTime = maxDelta / timePerAngle;
return(lm);
}
