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]); }