/** get all buttons from a gamepad */ public static void FillBtns(CTRE.Gamepad gamepad, ref bool[] btns, ref int pov) { gamepad.GetAllValues(ref _gamepadValues); pov = _gamepadValues.pov; for (uint i = 1; i < btns.Length; ++i) { btns[i] = gamepad.GetButton(i); } }
public void RunForever() { int toPrint = 0; int loops = 0; CTRE.TalonSrx _talon = new CTRE.TalonSrx(0); /* make a talon with deviceId 0 */ CTRE.PigeonImu _pigeon; /** Use a USB gamepad plugged into the HERO */ CTRE.Gamepad _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); CTRE.GamepadValues _gamepadValues = new CTRE.GamepadValues(); uint _oldBtns = 0; StringBuilder _sb = new StringBuilder(); /** choose pigeon connected to Talon or on CAN Bus */ if (true) { _pigeon = new CTRE.PigeonImu(0); /* Pigeon is on CAN Bus with device ID 0 */ } else { _pigeon = new CTRE.PigeonImu(_talon); /* Pigeon is connected to _talon via Gadgeteer cable.*/ } /* set frame rates */ _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.BiasedStatus_4_Mag, 17); _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_1_General, 217); _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_11_GyroAccum, 9999); _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_9_SixDeg_YPR, 0); _pigeon.SetAccumZAngle(217); /* loop forever */ while (true) { int ret = 0; /* signals for motion */ float[] ypr = new float[3]; float[] wxyz = new float[4]; float compassHeading, magnitudeMicroTeslas; float tempC; CTRE.PigeonImu.PigeonState state; Int16[] rmxyz = new Int16[3]; Int16[] bmxyz = new Int16[3]; Int16[] baxyz = new short[3]; Int16 baNorm; float[] xyz_dps = new float[3]; float [] tilts_XZ_YZ_XY = new float[3]; UInt32 timeSec; float fusedHeading; CTRE.PigeonImu.FusionStatus fusionStatus = new CTRE.PigeonImu.FusionStatus(); CTRE.PigeonImu.GeneralStatus generalStatus = new CTRE.PigeonImu.GeneralStatus(); /* get sigs */ ret |= _pigeon.GetYawPitchRoll(ypr); ret |= _pigeon.Get6dQuaternion(wxyz); compassHeading = _pigeon.GetCompassHeading(); ret |= _pigeon.GetLastError(); magnitudeMicroTeslas = _pigeon.GetCompassFieldStrength(); ret |= _pigeon.GetLastError(); tempC = _pigeon.GetTemp(); ret |= _pigeon.GetLastError(); state = _pigeon.GetState(); ret |= _pigeon.GetLastError(); ret |= _pigeon.GetRawMagnetometer(rmxyz); ret |= _pigeon.GetBiasedMagnetometer(bmxyz); ret |= _pigeon.GetBiasedAccelerometer(baxyz, out baNorm); ret |= _pigeon.GetRawGyro(xyz_dps); ret |= _pigeon.GetAccelerometerAngles(tilts_XZ_YZ_XY); timeSec = _pigeon.GetUpTime(); fusedHeading = _pigeon.GetFusedHeading(fusionStatus); ret |= _pigeon.GetGeneralStatus(generalStatus); /* get buttons */ _gamepad.GetAllValues(ref _gamepadValues); if (_gamepadValues.pov == 0) // up { _pigeon.EnableTemperatureCompensation(true); } if (_gamepadValues.pov == 4) // dn { _pigeon.EnableTemperatureCompensation(false); } if (_oldBtns != _gamepadValues.btns) { int btnIdx = GetButtonFromBits(_gamepadValues.btns); switch (btnIdx) { case 4: if (toPrint < kToPrintMaxInclus) { ++toPrint; } break; case 2: if (toPrint > kToPrintMin) { --toPrint; } break; case 5: // top left _pigeon.SetYaw(100); _pigeon.SetFusedHeading(100); break; case 7: // btm left break; case 6: // top right _pigeon.SetYawToCompass(); _pigeon.SetFusedHeadingToCompass(); break; case 8: // btm right _pigeon.AddYaw(45); _pigeon.AddYaw(45); break; case 10: // enter mag cal start btn _pigeon.EnterCalibrationMode(CTRE.PigeonImu.CalibrationMode.Temperature); break; case 9: // accel 0,0, _pigeon.EnterCalibrationMode(CTRE.PigeonImu.CalibrationMode.Temperature); break; } _oldBtns = _gamepadValues.btns; } if (++loops >= 20) { loops = 0; /* build row */ switch (toPrint) { case 0: _sb.Append("YPR:"); _sb.Append(ToString(ypr)); break; case 1: _sb.Append("FusedHeading:"); _sb.Append(fusedHeading); _sb.Append(" deg, "); _sb.Append(fusionStatus.description); break; case 2: _sb.Append("Compass:"******" deg, "); _sb.Append(magnitudeMicroTeslas); _sb.Append("uT"); break; case 3: _sb.Append("tempC:"); _sb.Append(tempC); _sb.AppendLine(); break; case 4: _sb.Append("PigeonState:"); _sb.Append(state); _sb.AppendLine(); break; case 5: _sb.Append("BiasedM:"); _sb.Append(ToString(bmxyz)); break; case 6: _sb.Append("RawM:"); _sb.Append(ToString(rmxyz)); break; case 7: _sb.Append("BiasedAccel:"); _sb.Append(ToString(baxyz)); break; case 8: _sb.Append("GyroDps:"); _sb.Append(ToString(xyz_dps)); break; case 9: _sb.Append("AccelTilts:"); _sb.Append(ToString(tilts_XZ_YZ_XY)); break; case 10: _sb.Append("Quat:"); _sb.Append(ToString(wxyz)); break; case 11: _sb.Append("UpTime:"); _sb.Append(timeSec); break; case 12: _sb.Append("GS:"); _sb.Append(generalStatus.description); break; case 13: _sb.Append("tempCompensationCount:"); _sb.Append(generalStatus.tempCompensationCount); _sb.Append(" tempC:"); _sb.Append(generalStatus.tempC); _sb.Append(" noMotionBiasCount:"); _sb.Append(generalStatus.noMotionBiasCount); break; } kToPrintMin = 0; kToPrintMaxInclus = 13; //_sb.Append(yaw); _sb.Append(","); _sb.Append(pitch); _sb.Append(","); _sb.Append(roll); _sb.AppendLine(); //_sb.Append(w); _sb.Append(","); _sb.Append(x); _sb.Append(","); _sb.Append(y); _sb.Append(","); _sb.Append(z); _sb.AppendLine(); //_sb.Append(compassHeading); _sb.Append(","); _sb.Append(magnitudeMicroTeslas); _sb.AppendLine(); //_sb.Append(tempC); _sb.AppendLine(); //_sb.Append(state); _sb.AppendLine(); //_sb.Append(rmx); _sb.Append(","); _sb.Append(rmy); _sb.Append(","); _sb.Append(rmz); _sb.AppendLine(); //_sb.Append(bmx); _sb.Append(","); _sb.Append(bmy); _sb.Append(","); _sb.Append(bmz); _sb.AppendLine(); //_sb.Append(bam); _sb.Append(","); _sb.Append(bay); _sb.Append(","); _sb.Append(baz); _sb.Append(","); _sb.Append(baNorm); _sb.AppendLine(); //_sb.Append(x_dps); _sb.Append(","); _sb.Append(y_dps); _sb.Append(","); _sb.Append(z_dps); _sb.AppendLine(); //_sb.Append(tiltXZ); _sb.Append(","); _sb.Append(tiltYZ); _sb.Append(","); _sb.Append(tiltXY); _sb.AppendLine(); //_sb.Append(timeSec); _sb.Append(","); //_sb.Append(fusedHeading); _sb.Append(","); _sb.Append(bIsValid); _sb.Append(","); _sb.Append(bIsFusing); _sb.AppendLine(); //_sb.Append(ret); _sb.Append(","); } /* print line and clear buffer */ if (_sb.Length != 0) { Debug.Print(_sb.ToString()); _sb.Clear(); } /* 10ms loop */ Thread.Sleep(10); if (_pigeon.HasResetOccured()) { int resetCount; int resetFlags; int firmVers; resetCount = _pigeon.GetResetCount(); resetFlags = _pigeon.GetResetFlags(); firmVers = _pigeon.GetFirmVers(); Debug.Print("Pigeon Reset detected..."); Debug.Print(" Pigeon Reset Count:" + resetCount.ToString("D")); Debug.Print(" Pigeon Reset Flags:" + resetFlags.ToString("X")); Debug.Print(" Pigeon FirmVers:" + firmVers.ToString("X")); Debug.Print(" Waiting for delay"); Thread.Sleep(3000); } } }