public static void Main() { /* Initialize Display */ CTRE.Gadgeteer.Module.BalanceDisplayModule.LabelSprite titleDisplay, pitchDisplay, outputDisplay, PIDBalanceDisplay, PIDVelocityDisplay, PIDDriveDisplay, PIDScalerDisplay, batteryDisplay, encoderDisplayLeft, encoderDisplayRight; /* State and battery display */ titleDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Red, 1, 1, 80, 15); batteryDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Green, 80, 1, 80, 15); /* Pitch and output display on top right */ pitchDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 1, 21, 80, 15); outputDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 80, 21, 80, 15); /* Gain Display at the bottom */ PIDScalerDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Yellow, 1, 41, 80, 15); PIDBalanceDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 61, 90, 15); PIDDriveDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 81, 90, 15); PIDVelocityDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 101, 90, 15); encoderDisplayRight = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 90, 50, 10, 15); encoderDisplayLeft = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 90, 90, 10, 15); /* Initialize drivetrain */ Hardware.leftTalon.ConfigPeakCurrentLimit(15); Hardware.rightTalon.ConfigPeakCurrentLimit(15); Hardware.leftTalon.ConfigContinuousCurrentLimit(15); Hardware.rightTalon.ConfigContinuousCurrentLimit(15); Hardware.leftTalon.ConfigPeakCurrentDuration(0); Hardware.rightTalon.ConfigPeakCurrentDuration(0); Hardware.leftTalon.EnableCurrentLimit(true); Hardware.rightTalon.EnableCurrentLimit(true); Hardware.leftGearbox.SetSensorPhase(false); Hardware.rightTalon.ConfigVelocityMeasurementPeriod(CTRE.Phoenix.MotorControl.VelocityMeasPeriod.Period_10Ms); Hardware.rightTalon.ConfigVelocityMeasurementWindow(32); Hardware.leftTalon.ConfigVelocityMeasurementPeriod(CTRE.Phoenix.MotorControl.VelocityMeasPeriod.Period_10Ms); Hardware.leftTalon.ConfigVelocityMeasurementWindow(32); Hardware.leftTalon.ConfigNeutralDeadband(0.001f); Hardware.rightTalon.ConfigNeutralDeadband(0.001f); /* Speed up Pigeon frames */ Hardware.leftTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrame.Status_2_Feedback0_, 10, 10); Hardware.rightTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrame.Status_2_Feedback0_, 10, 10); Hardware.leftTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_3_Quadrature, 2, 10); Hardware.rightTalon.SetStatusFramePeriod(CTRE.Phoenix.MotorControl.StatusFrameEnhanced.Status_3_Quadrature, 2, 10); Hardware.pidgey.SetStatusFramePeriod(CTRE.Phoenix.Sensors.PigeonIMU_StatusFrame.BiasedStatus_2_Gyro, 5, 10); Hardware.pidgey.SetStatusFramePeriod(CTRE.Phoenix.Sensors.PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, 10); CTRE.Phoenix.MotorControl.SensorCollection _leftSensors = Hardware.leftTalon.GetSensorCollection(); CTRE.Phoenix.MotorControl.SensorCollection _rightSensors = Hardware.rightTalon.GetSensorCollection(); float[] XYZ_Dps = new float[3]; float tempP = 0; float tempI = 0; float tempD = 0; byte CurrentPID = 0; float lastVelocity = 0; CTRE.MP.MotionProfiler velocityRamp = new CTRE.MP.MotionProfiler(1000000, 6000, 0, 50); //Essentially infinite jerk, with a control on acceleration CTRE.Phoenix.Stopwatch loopTimer = new CTRE.Phoenix.Stopwatch(); loopTimer.Start(); uint lastOuterLoopTime = loopTimer.DurationMs; uint lastInnerLoopTime = loopTimer.DurationMs; float angleSetpoint = 0; uint loopCounter = 0; bool trackingMode = false; bool tippedOver = false; uint tipStart = 0; uint tipEnd = 0; bool pointTurnMode = false; while (true) { if (loopTimer.DurationMs < lastOuterLoopTime + 50) { continue; } if (loopTimer.DurationMs - lastOuterLoopTime > 1000) { //Just reset the timer, it's way too far away now lastOuterLoopTime = loopTimer.DurationMs; } lastOuterLoopTime += 50; if (Hardware.Gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected) { CTRE.Phoenix.Watchdog.Feed(); } float stick = Hardware.Gamepad.GetAxis(1); float turn = Hardware.Gamepad.GetAxis(2); CTRE.Phoenix.Util.Deadband(ref stick); CTRE.Phoenix.Util.Deadband(ref turn); /* Grab values from gamepad */ turn *= 0.5f; int xOffset = 0; int targetHeight = 0; bool disableForward = true; CTRE.Phoenix.Pixy2CCC.Block b = Hardware.pixy.ccc.OffsetBlock(ref disableForward, ref xOffset, ref targetHeight, 50, 2); if (b != null && trackingMode && !manualMode) { if (!disableForward) { stick = (targetHeight - 25) * 0.015f; } turn = xOffset * 0.002f; } bool button10 = Hardware.Gamepad.GetButton(10); if (button10 && !lastButton10) { trackingMode = !trackingMode; Hardware.Display.Clear(); if (trackingMode) { Hardware.Display.AddRectSprite(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Blue, 0, 0, 80, 130); Hardware.Display.AddRectSprite(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Blue, 80, 0, 80, 130); } else { titleDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Red, 1, 1, 80, 15); batteryDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Green, 80, 1, 80, 15); /* Pitch and output display on top right */ pitchDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 1, 21, 80, 15); outputDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 80, 21, 80, 15); /* Gain Display at the bottom */ PIDScalerDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Yellow, 1, 41, 80, 15); PIDBalanceDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 61, 90, 15); PIDDriveDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 81, 90, 15); PIDVelocityDisplay = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White, 1, 101, 90, 15); encoderDisplayRight = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 90, 50, 10, 15); encoderDisplayLeft = Hardware.Display.AddLabelSprite(Hardware.bigFont, CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Cyan, 90, 90, 10, 15); } } lastButton10 = button10; /* Pitch offsetter */ bool button2 = Hardware.Gamepad.GetButton(2); if (button2 && !lastButton2) { pitchoffset = 0; pitchoffset = GetPitch(); } lastButton2 = button2; /* State control */ bool button1 = Hardware.Gamepad.GetButton(1); if (button1 && !lastButton1) { OperateState = !OperateState; Iaccum = 0; Iaccum_velocity = 0; } lastButton1 = button1; bool button4 = Hardware.Gamepad.GetButton(4); if (button4 && !lastButton4) { CurrentPID++; if (CurrentPID > 2) { CurrentPID = 0; } } lastButton4 = button4; if (!trackingMode) { if (CurrentPID == 0) { PIDcontrol_test(BalancePID); PIDBalanceDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White); PIDDriveDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange); PIDVelocityDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange); } else if (CurrentPID == 1) { PIDcontrol_test(ArbitraryGains); PIDBalanceDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange); PIDDriveDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White); PIDVelocityDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange); } else if (CurrentPID == 2) { PIDcontrol_test(VelocityPID); PIDBalanceDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange); PIDDriveDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Orange); PIDVelocityDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.White); } if (PIDcycle == 0) { PIDBalanceDisplay.SetText("B P: " + BalancePID.P); PIDDriveDisplay.SetText("D P: " + ArbitraryGains.P); PIDVelocityDisplay.SetText("V P: " + VelocityPID.P); } else if (PIDcycle == 1) { PIDBalanceDisplay.SetText("B I: " + BalancePID.I); PIDDriveDisplay.SetText("D I: " + ArbitraryGains.I); PIDVelocityDisplay.SetText("V I: " + VelocityPID.I); } else if (PIDcycle == 2) { PIDBalanceDisplay.SetText("B D: " + BalancePID.D); PIDDriveDisplay.SetText("D D: " + ArbitraryGains.D); PIDVelocityDisplay.SetText("V D: " + VelocityPID.D); } PIDScalerDisplay.SetText("" + inc_dec); batteryDisplay.SetText("Bat: " + Hardware.leftTalon.GetBusVoltage()); } /* If Pigeon is connected and operation state lko09 */ if (Hardware.pidgey.GetState() == CTRE.Phoenix.Sensors.PigeonState.Ready && OperateState == true) { manualMode = false; } else { manualMode = true; } /* Velocity setpoint pulled from gamepad throttle joystick */ float velocitySetpoint = velocityRamp.Set(-stick * 1000.00f); /* Get pitch angular rate */ Hardware.pidgey.GetRawGyro(XYZ_Dps); float pitchRate = -XYZ_Dps[1]; int leftVel = 0; _leftSensors.GetQuadratureVelocity(out leftVel); int rightVel = 0; _rightSensors.GetQuadratureVelocity(out rightVel); float tempVelocity = -(-leftVel + rightVel) * 10 * 180 / 1920; // u/100ms converted into DPS /* Compensate for pitch angular rate when finding velocity */ float robotVelocity = ((tempVelocity) + pitchRate); //Keep in dps for easy math if ((turn > 0.4 || turn < -0.4) && ((velocitySetpoint < 100 && velocitySetpoint > -100) || pointTurnMode)) { //I'm doing a point turn, lock until I let go of turn velocitySetpoint *= 0.1f; pointTurnMode = true; } else { pointTurnMode = false; } turn *= (float)Math.Abs(linearlyInterpolate(robotVelocity, 0, 2000, 1, 0f)); turn *= (float)Math.Max(linearlyInterpolate((float)Math.Abs(GetPitch()), 0, 15, 1, 0f), 0); float velocityError = velocitySetpoint - robotVelocity; if (!trackingMode) { if (leftVel > 0) { encoderDisplayLeft.SetText("+"); } else if (leftVel < 0) { encoderDisplayLeft.SetText("|"); } else { encoderDisplayLeft.SetText("o"); } if (rightVel < 0) { encoderDisplayRight.SetText("+"); } else if (rightVel > 0) { encoderDisplayRight.SetText("|"); } else { encoderDisplayRight.SetText("o"); } } //=============================================================================================================================================// //=============================================================================================================================================// Iaccum_velocity += (velocityError); Iaccum_velocity = CTRE.Phoenix.Util.Cap(Iaccum_velocity, accummax_velocity); float velocityDerivative = robotVelocity - lastVelocity; lastVelocity = robotVelocity; float pValue_vel = velocityError * VelocityPID.P; pValue_vel = CTRE.Phoenix.Util.Cap(pValue_vel, 20); float iValue_vel = Iaccum_velocity * VelocityPID.I / 10; iValue_vel = CTRE.Phoenix.Util.Cap(iValue_vel, 20); //Cap I value to 20 degrees float dValue_vel = velocityDerivative * VelocityPID.D; angleSetpoint = -(pValue_vel + iValue_vel + dValue_vel); angleSetpoint = CTRE.Phoenix.Util.Cap(angleSetpoint, 15); byte[] frame2 = new byte[8]; frame2[0] = (byte)((int)(robotVelocity * 10) >> 8); frame2[1] = (byte)((int)(robotVelocity * 10) & 0xFF); frame2[2] = (byte)((int)(velocitySetpoint / 10)); frame2[3] = (byte)((int)(angleSetpoint * 100) >> 8); frame2[4] = (byte)((int)(angleSetpoint * 100) & 0xFF); frame2[5] = (byte)((int)(pValue_vel * 10)); frame2[6] = (byte)((int)(iValue_vel * 10)); frame2[7] = (byte)((int)(dValue_vel * 10)); ulong data2 = (ulong)BitConverter.ToUInt64(frame2, 0); CTRE.Native.CAN.Send(8, data2, 8, 0); /* Balance PID, call 5 times per outer call */ //=============================================================================================================================================// //=============================================================================================================================================// float pitchError = 0; lastInnerLoopTime = loopTimer.DurationMs; loopCounter = 0; while (loopCounter < 5) { if (loopTimer.DurationMs < lastInnerLoopTime) { continue; } lastInnerLoopTime += 5; loopCounter++; Hardware.pidgey.GetRawGyro(XYZ_Dps); float currentAngularRate = XYZ_Dps[1] * 0.001f; //Scaled down for easier gain control float currentPitch = GetPitch(); float targetPitch = angleSetpoint; pitchError = targetPitch - currentPitch; Iaccum += pitchError; Iaccum = CTRE.Phoenix.Util.Cap(Iaccum, accummax); tempP = BalancePID.P * getGainTerm(robotVelocity); //Scale P term with velocity tempI = BalancePID.I / 10; tempD = BalancePID.D; if (pointTurnMode) { //We are point turning so zero I and Scalar P tempP = BalancePID.P; tempI = 0; } float pValue = (pitchError) * tempP; float iValue = (Iaccum) * tempI; float dValue = (currentAngularRate) * tempD; iValue = CTRE.Phoenix.Util.Cap(iValue, 0.3f); //Cap I to 30% of max motor output float Output = pValue - dValue + iValue; float outputVelocity = robotVelocity / 2000; float eventHorizonForward; float eventHorizonReverse; if (outputVelocity > 0) { eventHorizonForward = linearlyInterpolate(outputVelocity, 0, 1, 60, 30); eventHorizonReverse = linearlyInterpolate(outputVelocity, 0, 1, -60, -120); } else { eventHorizonForward = linearlyInterpolate(outputVelocity, 0, -1, 60, 120); eventHorizonReverse = linearlyInterpolate(outputVelocity, 0, -1, -60, -30); } if (-currentPitch > eventHorizonForward || -currentPitch < eventHorizonReverse) { tipEnd = loopTimer.DurationMs; } if (loopTimer.DurationMs - tipStart > 200) { tippedOver = true; } if (loopTimer.DurationMs - tipEnd > 1000) { tippedOver = false; } if (tippedOver) { Output = 0; Iaccum_velocity = 0; Iaccum = 0; manualMode = true; } /* Process output */ //=============================================================================================================================================// //=============================================================================================================================================// Output = CTRE.Phoenix.Util.Cap(Output, maxOutput); //cap value from [-1, 1] if (Hardware.battery.IsLow()) { /* Scale all drivetrain inputs to 25% if battery is low */ batteryDisplay.SetColor(CTRE.Gadgeteer.Module.BalanceDisplayModule.Color.Red); Output *= 0.25f; stick *= 0.25f; turn *= 0.25f; } if (manualMode == false) { /* In balance mode, use PI -> PID -> Output */ DrivetrainSet(Output, turn, false); if (!trackingMode) { titleDisplay.SetText("Enabled"); } if (!trackingMode) { outputDisplay.SetText("Out: " + Output); } } else { /* In maual mode/disabled, use joystick -> Output */ DrivetrainSet(stick, turn, true); if (!trackingMode) { titleDisplay.SetText("Disabled"); } if (!trackingMode) { outputDisplay.SetText("Out: " + stick); } } /* Balance CAN Frame */ byte[] frame = new byte[8]; frame[0] = (byte)((int)(pValue * 1000) >> 8); frame[1] = (byte)((int)(pValue * 1000) & 0xFF); frame[2] = (byte)((int)(-dValue * 100000) >> 8); frame[3] = (byte)((int)(-dValue * 100000) & 0xFF); frame[4] = (byte)((int)(iValue * 1000) >> 8); frame[5] = (byte)((int)(iValue * 1000) & 0xFF); frame[6] = (byte)((int)(Output * 1000) >> 8); frame[7] = (byte)((int)(Output * 1000) & 0xFF); ulong data = (ulong)BitConverter.ToUInt64(frame, 0); CTRE.Native.CAN.Send(9, data, 8, 0); } if (!trackingMode) { pitchDisplay.SetText("P: " + GetPitch()); } //Thread.Sleep(5); } }
public void Calculate(double throttle, double moveVal, double turnVal) { stopwatch = new CTRE.Phoenix.Stopwatch(); stopwatch.Start(); this.Led.FlashyLed = false; CalculateRpm(); CalculateSpinTimes(moveVal, turnVal); FullPowerSpin = false; if (CurrentRpm < MinRpm) { FullPowerSpin = true; } if (CurrentAccel > MaxAccel || CurrentRpm > MaxRpm) { throttle = .1; } if (throttle > .5) { this.Led.FlashyLed = true; BrakingLength = HalfSpinTimeMs * 25; BeginBrake = HalfSpinTimeMs / 2 - BrakingLength; EndBrake = HalfSpinTimeMs / 2 + BrakingLength; if (BeginBrake < 1) { BeginBrake = 1; } PowerKillPart1 = 0; PowerKillPart2 = HalfSpinTimeMs; } if (throttle <= .5) { BeginBrake = 1; EndBrake = HalfSpinTimeMs; PowerKillLength = (50 - throttle * 100) * HalfSpinTimeMs / 150; PowerKillPart1 = PowerKillLength; PowerKillPart2 = HalfSpinTimeMs - PowerKillLength; } if (FullPowerSpin) { EndBrake = 1; BeginBrake = 0; PowerKillPart1 = 0; PowerKillPart2 = HalfSpinTimeMs; } this.Throttle = throttle; this.MoveVal = moveVal; this.TurnVal = TurnVal; }
public Pixy2CCC(TPixy2 pixy) { m_pixy = pixy; blocks = new Block[64]; m_timeSinceLastGoodBlock = new CTRE.Phoenix.Stopwatch(); }
public Orient() { orientState = OrientState.INIT; stopwatch = new CTRE.Phoenix.Stopwatch(); stopwatch.Start(); }