private void OnOrient(MessageOrient data)
{
//increases to pos max,
//decreases to negative min
checkForDebugMovement(data);
bool forwards = true;
Vector3 curOrientation = new Vector3(data.alpha, data.beta, data.gamma);
//curOrientation = fixAngles(curOrientation);
avWindow.Add(curOrientation);
int smoothWindowSize = 8;
if (avWindow.Count >= smoothWindowSize)
{
//avWindow.Clear();
if (avWindow.Count > smoothWindowSize)
{
avWindow.RemoveAt(0);
}
curOrientation = Vector3.zero;
for (int i = 0; i < avWindow.Count; i++ )
{
curOrientation += avWindow[i];
}
curOrientation = curOrientation * (800.0f / avWindow.Count);
for (int i = 0; i < 3; i++)
{
if (Mathf.Abs(curOrientation[i]) > 2)
{
curOrientation[i] *= 1.0f / Mathf.Abs(curOrientation[i]);
}
else
{
curOrientation[i] = 0;
}
}
if (curOrientation != prevOrientation)
{
//print(" " + curOrientation.x + " " + curOrientation.y + " " + curOrientation.z);
}
}
// float accThresh = 100;
// if (Mathf.Abs(data.alpha) > accThresh ||
// Mathf.Abs(data.beta) > accThresh ||
// Mathf.Abs(data.gamma) > accThresh)
// {
/*for (int i = 0; i < 3; i++ )
{
if (Mathf.Abs(curOrientation[i]) <= accThresh)
{
curOrientation[i] = 0;
}
else
{
curOrientation[i] /= Mathf.Abs(curOrientation[i]);
}
}*/
// }
float[] xSigns = { 1, -1, -1, 1 };
float[] zSigns = { 1,1,-1,-1};
int stateForward = (windmillState + 1) % 4;
int stateBack = (windmillState - 1 + 4) % 4;
bool hitStateForwards = curOrientation.x == xSigns[stateForward] && curOrientation.z == zSigns[stateForward];
bool hitStateBackwards = curOrientation.x == xSigns[stateBack] && curOrientation.z == zSigns[stateBack];
if (hitStateForwards)
{
if (windmillDir >= 2)
{
windmillState = stateForward;
print("advance");
if (this.isHorizontalController)
{
myCopter.thrustHorizontal(-1.0f);
//print(quadrantIndex + " right");
}
else
{
if (myCopter != null)
{
//myCopter.thrustUp();
myCopter.thrustDown();
}
}
}
windmillDir = Mathf.Min(windmillDir + 1, 4);
}
else if (hitStateBackwards || data.alpha == -999)
{
if (windmillDir <= -2)
{
windmillState = stateBack;// (windmillState + 1) % 4;
print("retreat");
if (this.isHorizontalController)
{
myCopter.thrustHorizontal(1.0f);
//print(quadrantIndex + " right");
}
else
{
if (myCopter != null)
{
myCopter.thrustUp();
}
}
}
windmillDir = Mathf.Max(windmillDir - 1, -4);
}
else
{
windmillDir = windmillDir < 0 ? windmillDir + 1 : windmillDir - 1;
}
prevOrientation = curOrientation;
//The old method... needs speed increased in copter
//checkPhoneSpinOnTable(data);
}
void checkPhoneSpinOnTable(MessageOrient data)
{
float angDiff = previousAlpha - data.beta;
if (angDiff > 180)
{
angDiff -= 360;
}
if (angDiff < -180)
{
angDiff += 360;
}
//print(angDiff);
//int range = 90;
/*int checkRange = 45;
int passAngle = 90;
int minAngle = passAngle - checkRange;
int maxAngle = passAngle + checkRange;
bool pastAngle = previousAlpha >= minAngle && data.alpha <= maxAngle;*/
bool[] pastQuadrantArray =
{
previousAlpha > 0 && data.alpha < 180, // passing 90
previousAlpha > 90 && data.alpha < 270, // passing 180
previousAlpha > 180 && data.alpha < 259, // passing 270
previousAlpha > 270 && data.alpha < 90, // passing zero
};
bool[] pastQuadrantArrayBackwards =
{
data.alpha > 0 && previousAlpha < 180, // passing 90
data.alpha > 90 && previousAlpha < 270, // passing 180
data.alpha > 180 && previousAlpha < 259, // passing 270
data.alpha > 270 && previousAlpha < 90, // passing zero
};
if ((pastQuadrantArray[quadrantIndex] && angDiff < 0) || data.alpha == 999) //ccw
{
quadrantIndex = (quadrantIndex + 1) % pastQuadrantArray.Length;
//print(quadrantIndex);
//print("rpm : " + rpm * 60 + " _ spins : " + spinCount);
//this.rigidbody.AddForce(Vector3.up * upSpeed, ForceMode.Acceleration);
//!!!
if (this.isHorizontalController)
{
myCopter.thrustHorizontal(1.0f);
//print(quadrantIndex + " right");
}
else
{
if (myCopter != null)
{
myCopter.thrustUp();
}
}
if (quadrantIndex == 0)
{
spinCount++;
}
}
else if ((pastQuadrantArrayBackwards[quadrantIndex] && angDiff > 0) || data.alpha == -999)
{
if (this.isHorizontalController)
{
myCopter.thrustHorizontal(-1.0f);
//print(quadrantIndex + " left");
}
quadrantIndex = ((quadrantIndex - 1) + pastQuadrantArray.Length) % pastQuadrantArray.Length;
//print(quadrantIndex);
if (quadrantIndex == 0)
{
spinCount--;
}
}
previousAlpha = data.beta;
}
void HandleOrient(MessageOrient data)
{
axes[AXIS_ORIENTATION_ALPHA] = data.a; // data.a / 180 - 1; // range is suspposed to be 0 to 359
axes[AXIS_ORIENTATION_BETA] = data.b; // data.b / 180; // range is suspposed to be -180 to 180
axes[AXIS_ORIENTATION_GAMMA] = data.g; // data.g / 90; // range is suspposed to be -90 to 90
}
void checkForDebugMovement(MessageOrient data)
{
if (myCopter != null && Mathf.Abs(data.alpha) == 777)
{
myCopter.scored();
}
if (myCopter != null && Mathf.Abs(data.alpha) == 999)
{
if (this.isHorizontalController)
{
if (data.alpha == 999)
{
myCopter.thrustHorizontal(1.0f);
}
else
{
myCopter.thrustHorizontal(-1.0f);
}
//print(quadrantIndex + " right");
}
else
{
if (data.alpha == 999)
{
myCopter.thrustUp();
}
else
{
myCopter.thrustDown();
}
}
}
}
void HandleOrient(MessageOrient data)
{
axes[AXIS_ORIENTATION_ALPHA] = data.a; // data.a / 180 - 1; // range is suspposed to be 0 to 359
axes[AXIS_ORIENTATION_BETA] = data.b; // data.b / 180; // range is suspposed to be -180 to 180
axes[AXIS_ORIENTATION_GAMMA] = data.g; // data.g / 90; // range is suspposed to be -90 to 90
}
