void Loop10Ms() { /* get all the buttons */ FillBtns(ref _btns); /* scale the x-axis to go from 0 to sensorRange, left to right */ float leftAxisX = (((_sensorRange / 2) * _gamepad.GetAxis(0)) + (_sensorRange / 2)); float rightAxisX = kJoystickScaler * _gamepad.GetAxis(2); Deadband(ref rightAxisX); if (rightAxisX != 0) { _talon.SetControlMode(ControlMode.kPercentVbus); _talon.Set(rightAxisX); } else if (_talon.GetControlMode() == ControlMode.kPercentVbus) { _targetPosition = _talon.GetPosition(); /* user has let go of the stick, lets closed-loop whereever we happen to be */ EnableClosedLoop(); } /* When you press the 'A' button on a Logitech Gamepad * and the enable button is pressed */ if (_btns[2] && !_btnsLast[2] && _gamepad.GetButton(kEnableButton)) { _targetPosition = servo(leftAxisX, _talon.GetPosition(), _sensorRange); EnableClosedLoop(); } }
/** * Mecanum Drive that is inverted on the left side and decreases output when low battery * * @param Forward Forward/Backward drive of mecanum drive * @param Strafe Left/Right drive of mecanum drive * @param Twist Turn left/Right of mecanum drive */ private static void MecanumDrive(float Forward, float Strafe, float Twist) { float leftFrnt = (Forward + Strafe + Twist); // left front moves positive for forward, strafe-right, turn-right float leftRear = (Forward - Strafe + Twist); // left rear moves positive for forward, strafe-left, turn-right float rghtFrnt = (Forward - Strafe - Twist); // right front moves positive for forward, strafe-left, turn-left float rghtRear = (Forward + Strafe - Twist); // right rear moves positive for forward, strafe-right, turn-left /* Invert left sided motors */ leftFrnt *= -1; leftRear *= -1; /* If battery is lower than 10% scale down output */ if (_Battery.IsLow()) { leftFrnt *= 0.5f; leftRear *= 0.5f; rghtFrnt *= 0.5f; rghtRear *= 0.5f; } /* Feed Talons */ RightFront.Set(rghtFrnt); RightRear.Set(rghtRear); LeftFront.Set(leftFrnt); LeftRear.Set(leftRear); }
void initTurret() { /* first choose the sensor */ _turret.SetFeedbackDevice(TalonSrx.FeedbackDevice.CtreMagEncoder_Relative); _turret.SetSensorDirection(false); _turret.ConfigEncoderCodesPerRev(4096); // if using CTRE.TalonSrx.FeedbackDevice.QuadEncoder _turret.SetP(0, TURRET_P); /* tweak this first, a little bit of overshoot is okay */ _turret.SetI(0, TURRET_I); _turret.SetD(0, TURRET_D); _turret.SetF(0, TURRET_F); /* use slot0 for closed-looping */ _turret.SelectProfileSlot(0); /* set the peak and nominal outputs, 12V means full */ _turret.ConfigNominalOutputVoltage(MINIMUM_TURRET_VOLTAGE, -1 * MINIMUM_TURRET_VOLTAGE); //The minimum voltage that will be applied to the turret. _turret.ConfigPeakOutputVoltage(+3.0f, -3.0f); //THe maximum voltage that will be applied to the turret. /* how much error is allowed? This defaults to 0. */ _turret.SetAllowableClosedLoopErr(0, 0); _turret.SetPosition(0); /* start our position at zero, this example uses relative positions */ _turret.SetVoltageRampRate(0); /* V per sec */ _turret.SetControlMode(ControlMode.kPosition); _turret.Set(angleSetpoint); _turret.SetEncPosition(0); }
/** * Zero the sensor and zero the throttle. */ void ZeroSensorAndThrottle() { _talon.SetPosition(0); /* start our position at zero, this example uses relative positions */ _targetPosition = 0; /* zero throttle */ _talon.SetControlMode(ControlMode.kPercentVbus); _talon.Set(0); Thread.Sleep(100); /* wait a bit to make sure the Setposition() above takes effect before sampling */ }
void Drive() { FillBtns(ref _btns); float y = -1 * _gamepad.GetAxis(1); Deadband(ref y); _talon.ProcessMotionProfileBuffer(); /* button handler, if btn5 pressed launch MP, if btn7 pressed, enter voltage mode */ if (_btns[5] && !_btnsLast[5]) { /* disable MP to clear IsLast */ _talon.SetControlMode(ControlMode.kMotionProfile); _talon.Set(0); CTRE.Watchdog.Feed(); Thread.Sleep(10); /* buffer new pts in HERO */ TalonSrx.TrajectoryPoint point = new TalonSrx.TrajectoryPoint(); _talon.ClearMotionProfileHasUnderrun(); _talon.ClearMotionProfileTrajectories(); for (uint i = 0; i < MotionProfile.kNumPoints; ++i) { point.position = (float)MotionProfile.Points[i][0]; point.velocity = (float)MotionProfile.Points[i][1]; point.timeDurMs = MotionProfile.kDurationMs; point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false; point.zeroPos = (i == 0) ? true : false; point.velocityOnly = false; point.profileSlotSelect = 0; _talon.PushMotionProfileTrajectory(point); } /* send the first few pts to Talon */ for (int i = 0; i < 5; ++i) { CTRE.Watchdog.Feed(); Thread.Sleep(10); _talon.ProcessMotionProfileBuffer(); } /*start MP */ _talon.Set(1); } else if (_btns[7] && !_btnsLast[7]) { _talon.SetControlMode(ControlMode.kVoltage); } /* if in voltage mode, update output voltage */ if (_talon.GetControlMode() == ControlMode.kVoltage) { _talon.Set(14.0f * y); } /* copy btns => btnsLast */ System.Array.Copy(_btns, _btnsLast, _btns.Length); }
/** spin in this routine forever */ public void RunForever() { /* config our talon, don't continue until it's successful */ int initStatus = SetupConfig(); /* configuration */ while (initStatus != 0) { Instrument.PrintConfigError(); initStatus = SetupConfig(); /* (re)config*/ } /* robot loop */ while (true) { /* get joystick params */ float leftY = -1f * _gamepad.GetAxis(1); bool btnTopLeftShoulder = _gamepad.GetButton(5); bool btnBtmLeftShoulder = _gamepad.GetButton(7); Deadband(ref leftY); /* keep robot enabled if gamepad is connected and in 'D' mode */ if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) { CTRE.Watchdog.Feed(); } /* set the control mode based on button pressed */ if (btnTopLeftShoulder) { _mode = ControlMode.kPercentVbus; } if (btnBtmLeftShoulder) { _mode = ControlMode.kMotionMagic; } /* calc the Talon output based on mode */ if (_mode == ControlMode.kPercentVbus) { float output = leftY; // [-1, +1] percent duty cycle _talon.SetControlMode(_mode); _talon.Set(output); } else if (_mode == ControlMode.kMotionMagic) { float servoToRotation = leftY * 10;// [-10, +10] rotations _talon.SetControlMode(_mode); _talon.Set(servoToRotation); } /* instrumentation */ Instrument.Process(_talon); /* wait a bit */ System.Threading.Thread.Sleep(5); } }
public void ServoToSpeed(float speedRPM) { /* close loop constants */ _ServoParameters.P = 0.01f; _ServoParameters.I = 0 * 0.0001f; _ServoParameters.F = 6f / 2000f; // about 6V for 2000 RPM /* save the target */ _targetSpeedRPM = speedRPM; /* get measured speed */ float measuredSpeedRpm = MeasuredSpeed; /* robot controller level closed loop, replace with firmware close loop later */ float output = _ServoParameters.PID(_targetSpeedRPM, measuredSpeedRpm, 0); _tal.Set(output); }
public static void Main() { TalonSrx test = new TalonSrx(0); GameController stick = new GameController(UsbHostDevice.GetInstance(0), 0); /* loop forever */ while (true) { if (stick.GetConnectionStatus() == UsbDeviceConnection.Connected) { CTRE.Watchdog.Feed(); } //This call is redundant but you can un-comment to guarantee limit switches will work or change the mode. //test.ConfigLimitMode(TalonSrx.LimitMode.kLimitMode_SwitchInputsOnly); Debug.Print("Rev: " + test.IsRevLimitSwitchClosed() + " | Fwd: " + test.IsFwdLimitSwitchClosed()); Debug.Print("" + test.GetFaults()); if (stick.GetButton(1)) { test.ClearStickyFaults(); } test.Set(stick.GetAxis(1)); /* wait a bit */ System.Threading.Thread.Sleep(10); } }
void initShooter() { /* first choose the sensor */ _shooter.SetFeedbackDevice(TalonSrx.FeedbackDevice.CtreMagEncoder_Relative); _shooter.SetSensorDirection(false); _shooter.SetControlMode(ControlMode.kSpeed); _shooter.ConfigNominalOutputVoltage(+0.65f, -0.65f); //The minimum voltage that will be applied to the shooter. _shooter.ConfigPeakOutputVoltage(+12.0f, -12.0f); //THe maximum voltage that will be applied to the shooter. _shooter.Set(0); _shooter.SetP(0, SHOOTER_P); /* tweak this first, a little bit of overshoot is okay */ _shooter.SetI(0, SHOOTER_I); _shooter.SetD(0, SHOOTER_D); _shooter.SetF(0, SHOOTER_F); /* use slot0 for closed-looping */ _shooter.SelectProfileSlot(0); }
static void drive(float x, float y, float turn) { float[] ypr = new float[3]; pigeon.GetYawPitchRoll(ypr); float turnPower = turnPID(turn - ypr[0]); leftF.Set((y - x + turnPower)); leftB.Set((y + x + turnPower)); rightB.Set(-(y - x - turnPower)); rightF.Set(-(y + x - turnPower)); }
static void Run() { float x = _gamepad.GetAxis(1); Deadband(ref x); //Set the Maximum Current Limit for the Talon (in Amps) talon.SetCurrentLimit(10); //Enable the Current Limiting Feature. talon.EnableCurrentLimit(true); talon.Set(x); float current = talon.GetOutputCurrent(); stringBuilder.Append("\t"); stringBuilder.Append(current); stringBuilder.Append("\t"); }
public static void Main() { RCDGamepad gamepad = new RCDGamepad(new CTRE.UsbHostDevice()); TalonSrx driveTalon = new TalonSrx(1); driveTalon.SetInverted(true); driveTalon.SetCurrentLimit(1); driveTalon.EnableCurrentLimit(true); driveTalon.ConfigNeutralMode(TalonSrx.NeutralMode.Coast); TalonSrx flywheelTalon = new TalonSrx(2); flywheelTalon.SetCurrentLimit(40); flywheelTalon.EnableCurrentLimit(true); flywheelTalon.ConfigNeutralMode(TalonSrx.NeutralMode.Coast); PigeonImu centerOfMassIMU = new PigeonImu(1); PigeonImu headIMU = new PigeonImu(2); uint pulsePeriod = 20000; uint pulseDuration = 1500; PWM pwm_tilt = new PWM(CTRE.HERO.IO.Port3.PWM_Pin9, pulsePeriod, pulseDuration, PWM.ScaleFactor.Microseconds, false); PWM pwm_headRoll = new PWM(CTRE.HERO.IO.Port3.PWM_Pin7, pulsePeriod, pulseDuration, PWM.ScaleFactor.Microseconds, false); PWM pwm_headPitch = new PWM(CTRE.HERO.IO.Port3.PWM_Pin6, pulsePeriod, pulseDuration, PWM.ScaleFactor.Microseconds, false); PWM pwm_headSpin = new PWM(CTRE.HERO.IO.Port3.PWM_Pin8, pulsePeriod, pulseDuration, PWM.ScaleFactor.Microseconds, false); PWM pwm_disco = new PWM(CTRE.HERO.IO.Port3.PWM_Pin4, pulsePeriod, pulseDuration, PWM.ScaleFactor.Microseconds, false); pwm_tilt.Start(); pwm_headRoll.Start(); pwm_headPitch.Start(); pwm_headSpin.Start(); pwm_disco.Start(); RCDTalonSpeedController driveSpeedController = new RCDTalonSpeedController("Primary Drive"); driveSpeedController.loggingEnabled = false; driveSpeedController.rampingEnabled = false; bool previousButtonX = false; bool previousButtonY = false; bool previousButtonB = false; bool previousButtonA = false; bool previousStickButtonL = false; bool previousStickButtonR = false; bool staticOperationMode = false; ConfigureBodyLightColors(); int orientationCompensation = -1; while (true) { if (gamepad.GetConnectionStatus() != UsbDeviceConnection.Connected) { continue; } CTRE.Watchdog.Feed(); // Check for whether we need to flip orientation bool currentButtonY = gamepad.buttonY; if (!currentButtonY && previousButtonY) { orientationCompensation *= -1; // No (working) interface for haptic feedback currently exists // RumbleForDuration(gamepad, 0.25); } previousButtonY = currentButtonY; // Check for whether we need to switch control modes between driving and static modes bool currentButtonB = gamepad.buttonB; if (!currentButtonB && previousButtonB) { staticOperationMode = !staticOperationMode; // No (working) interface for haptic feedback currently exists // RumbleForDuration(gamepad, 0.25); } previousButtonB = currentButtonB; uint discoDuration = 0; // Check for whether we need to send happy beeps bool currentButtonA = gamepad.buttonA; if (currentButtonA) { discoDuration = 1500; } previousButtonA = currentButtonA; // Check if we need to send sad beeps bool currentButtonX = gamepad.buttonX; if (currentButtonX) { discoDuration = 2500; } previousButtonX = currentButtonX; // Check if we need to send volume down bool currentStickButtonL = gamepad.buttonLeftJoyClick; if (currentStickButtonL) { discoDuration = 3500; } previousStickButtonL = currentStickButtonL; // Check if we need to send volume up bool currentStickButtonR = gamepad.buttonRightJoyClick; if (currentStickButtonR) { discoDuration = 4500; } previousStickButtonR = currentStickButtonR; pwm_disco.Duration = discoDuration; /* Capture button states for gamepad */ Vector leftVec = gamepad.leftVector; Vector rightVec = gamepad.rightVector; bool leftBumper = gamepad.leftBumper; bool rightBumper = gamepad.rightBumper; bool leftTrigger = gamepad.leftTrigger; bool rightTrigger = gamepad.rightTrigger; uint servoZero = 1500; pwm_headRoll.Duration = (uint)((rightVec.x * -600 * orientationCompensation) + servoZero); pwm_headPitch.Duration = (uint)((rightVec.y * 600 * orientationCompensation) + servoZero); if (staticOperationMode) { // Not driving or tilting while in static mode driveTalon.Set(0); pwm_tilt.Duration = servoZero; pwm_headSpin.Duration = (uint)((leftVec.x * 600 * orientationCompensation) + servoZero); } else { // Drive forward/backward float driveValue = leftVec.y * orientationCompensation; driveValue = driveSpeedController.ComputeCurrentValue(driveValue); driveTalon.Set(driveValue); uint headSpinScalar = 500; uint headSpinPosition = servoZero; headSpinPosition += (uint)(leftTrigger ? -headSpinScalar * orientationCompensation : 0); headSpinPosition += (uint)(rightTrigger ? headSpinScalar * orientationCompensation : 0); pwm_headSpin.Duration = headSpinPosition; pwm_tilt.Duration = (uint)((leftVec.x * 600 * orientationCompensation) + servoZero); } /* Throttle buttons are additive so they cancel if pressed simultaneously */ float flywheelThrottle = 0; float flywheelMagnitude = 1.0F; flywheelThrottle += leftBumper ? flywheelMagnitude : 0; flywheelThrottle += rightBumper ? -flywheelMagnitude : 0; flywheelTalon.Set(flywheelThrottle); // No current (working) interface for Logitech remote's rumble feature. // Commenting out so we don't waste cycles // UpdateRumbleState(gamepad); // Wait a bit System.Threading.Thread.Sleep(5); } }
public static void Main() { Arduino arduino = new Arduino(CTRE.HERO.IO.Port1.UART, 9600); TalonSrx talon = new TalonSrx(1); TalonSrx talon2 = new TalonSrx(2); OI controller = new OI(new Gamepad(new UsbHostDevice())); int motorSpeed = 0; int motorStoredSpeed = 700; float rMotorSpeed = 0f; float rMotorStoreSpeed = .7f; bool isPressed = false; byte tData; int tMotorSpeed; int counter; while (!arduino.Status()) { Debug.Print("Waiting for arduino"); } while (true) { if (controller.GetButton(OI.Button.a) && !isPressed) { motorSpeed += 50; rMotorSpeed += .05f; } else if (controller.GetButton(OI.Button.b) && !isPressed) { motorSpeed -= 50; rMotorSpeed -= .05f; } if (controller.GetButton(OI.Button.x) && !isPressed) { motorSpeed += 10; rMotorSpeed += .01f; } else if (controller.GetButton(OI.Button.y) && !isPressed) { motorSpeed -= 10; rMotorSpeed -= .01f; } if (controller.GetButton(OI.Button.a) || controller.GetButton(OI.Button.b) || controller.GetButton(OI.Button.x) || controller.GetButton(OI.Button.y)) { isPressed = true; } else { isPressed = false; } if (controller.GetAxis(OI.Axis.left_y) == -1) { motorSpeed = 0; rMotorSpeed = 0; } else if (controller.GetAxis(OI.Axis.left_y) == 1) { motorSpeed = motorStoredSpeed; rMotorSpeed = rMotorStoreSpeed; } tMotorSpeed = motorSpeed / 10; tData = (byte)(tMotorSpeed - (tMotorSpeed % 10)); //Outputs 100 place of motor speed tData += (byte)(tMotorSpeed - tData); if (controller.GetAxis(OI.Axis.right_y) == -1 && (counter % 1000) == 0) { arduino.sendCommand('U', 'P', tData, talon.GetOutputVoltage(), (byte)talon.GetOutputCurrent()); } else if (controller.GetAxis(OI.Axis.right_y) == 1 && (counter % 1000) == 0) { arduino.sendCommand('D', 'O', tData, talon.GetOutputVoltage(), (byte)talon.GetOutputCurrent()); } else if ((counter % 1000) == 0) { arduino.sendCommand('0', '0', tData, talon.GetOutputVoltage(), (byte)talon.GetOutputCurrent()); } talon.Set(rMotorSpeed); talon2.Set(rMotorSpeed); CTRE.Watchdog.Feed(); Debug.Print("Motor Speed = " + motorSpeed); Debug.Print("tData = " + tData); counter++; } }
public void RunForever() { /* enable XInput, if gamepad is in DInput it will disable robot. This way you can * use X mode for drive, and D mode for disable (instead of vice versa as the * stock HERO implementation traditionally does). */ while (true) { if (_gamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) { CTRE.Watchdog.Feed(); } /* get buttons */ bool[] btns = new bool[_buttons.Length]; for (uint i = 1; i < 20; ++i) { btns[i] = _gamepad.GetButton(i); } /* get sticks */ for (uint i = 0; i < _sticks.Length; ++i) { _sticks[i] = _gamepad.GetAxis(i); } /* yield for a bit, and track timeouts */ System.Threading.Thread.Sleep(10); if (_rumblingTimeMs < 5000) { _rumblingTimeMs += 10; } /* update the Talon using the shoulder analog triggers */ _tal.Set((_sticks[5] - _sticks[4]) * 0.60f); /* fire some solenoids based on buttons */ _driver.Set(1, _buttons[1]); _driver.Set(2, _buttons[2]); _driver.Set(3, _buttons[3]); _driver.Set(4, _buttons[4]); /* rumble state machine */ switch (_rumblinSt) { /* rumbling is disabled, require some off time to save battery */ case 0: _gamepad.SetLeftRumble(0); _gamepad.SetRightRumble(0); if (_rumblingTimeMs < 100) { /* waiting for off-time */ } else if ((btns[1] && !_buttons[1])) /* button off => on */ { /* off time long enough, user pressed btn */ _rumblingTimeMs = 0; _rumblinSt = 1; _gamepad.SetLeftRumble(0xFF); } else if ((btns[2] && !_buttons[2])) /* button off => on */ { /* off time long enough, user pressed btn */ _rumblingTimeMs = 0; _rumblinSt = 1; _gamepad.SetRightRumble(0xFF); } break; /* already vibrating, track the time */ case 1: if (_rumblingTimeMs > 500) { /* vibrating too long, turn off now */ _rumblingTimeMs = 0; _rumblinSt = 0; _gamepad.SetLeftRumble(0); _gamepad.SetRightRumble(0); } else if ((btns[3] && !_buttons[3])) /* button off => on */ { /* immedietely turn off */ _rumblingTimeMs = 0; _rumblinSt = 0; _gamepad.SetLeftRumble(0); _gamepad.SetRightRumble(0); } else if ((btns[1] && !_buttons[1])) /* button off => on */ { _gamepad.SetLeftRumble(0xFF); } else if ((btns[2] && !_buttons[2])) /* button off => on */ { _gamepad.SetRightRumble(0xFF); } break; } /* this will likley be replaced with a strongly typed interface, * control the LEDs on the center XBOX emblem. */ if (btns[5] && !_buttons[5]) { _gamepad.SetLEDCode(6); } if (btns[6] && !_buttons[6]) { _gamepad.SetLEDCode(7); } if (btns[7] && !_buttons[7]) { _gamepad.SetLEDCode(8); } if (btns[8] && !_buttons[8]) { _gamepad.SetLEDCode(9); } /* build line to print */ StringBuilder sb = new StringBuilder(); foreach (float stick in _sticks) { sb.Append(Format(stick)); sb.Append(","); } sb.Append("-"); for (uint i = 1; i < _buttons.Length; ++i) { if (_buttons[i]) { sb.Append("b" + i + ","); } } /* print useful info */ sb.AppendLine(); Debug.Print(sb.ToString()); /* save button states for button-change states */ _buttons = btns; } }