public float Get_heading() //Get_heading is to find yaw, pitch and roll
{
float[] ypr = new float[3];
drive_assister.GetYawPitchRoll(ypr);
//Finds the yaw, pitch and roll of the bot
return(ypr[0]);
//Sends # back up to Get_heading to beacome a float
}
public void Drive(Vector2 translationVelocity, double turn)
{
double rotationalVelocity = turn * TURN_SCALING;
gyro.GetYawPitchRoll(gyroAngles);
double gyroAngle = gyroAngles[0];
// adjust translationVelocity by gyroAngle before adding the rotation to it
double translationSize = translationVelocity.Length;
double translationAngle = Units.ToDegrees(Math.Atan2(translationVelocity.X, -translationVelocity.Y));
//Microsoft.SPOT.Debug.Print("stickAngle: " + translationAngle + "\tGyro: " + gyroAngle);
translationAngle -= gyroAngle;
translationVelocity.X = Math.Sin(Units.ToRadians(translationAngle)) * translationSize;
translationVelocity.Y = -Math.Cos(Units.ToRadians(translationAngle)) * translationSize;
//Microsoft.SPOT.Debug.Print("X: " + translationVelocity.X + "\tY: " + translationVelocity.Y);
double max = 0.0;
foreach (var caster in casters)
{
double power = caster.ComputePowers(translationVelocity, rotationalVelocity, gyroAngle);
if (power > max)
{
max = power;
}
}
if (max < 1.0)
{
max = 1.0;
}
foreach (var caster in casters)
{
caster.ApplyPowers(max);
}
}
/** Return the heading from the Pigeon*/
public float GetImuHeading()
{
_pidgey.GetYawPitchRoll(YPR);
return(YPR[0]);
}