//I think this is where the magic happens and the motors move private static void processInboundData(ArrayList setpointDataList, ArrayList talons) { //For each setpointData, for the talon that matches it set each mode and setpoint based on the list information for (int i = 0; i < setpointDataList.Count; i++) { try { SetpointData setpointData = (SetpointData)setpointDataList[i]; float setpointVal = (float)(setpointData.getSetpoint()); CTRE.TalonSrx talon = (CTRE.TalonSrx)talons[i]; if (talon.GetControlMode() != setpointData.getMode()) { talon.SetControlMode(setpointData.getMode()); //Debug.Print(setpointData.getMode().ToString()); } if (talon.GetSetpoint() != setpointVal) { talon.Set(setpointVal); Debug.Print("Setting it to value = " + setpointVal.ToString()); //Debug.Print(setpointVal.ToString()); } } catch (ArgumentOutOfRangeException ex) { Debug.Print(ex.ToString()); } } }
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(CTRE.TalonSrx.ControlMode.kMotionProfile); _talon.Set(0); CTRE.Watchdog.Feed(); Thread.Sleep(10); /* buffer new pts in HERO */ CTRE.TalonSrx.TrajectoryPoint point = new CTRE.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(CTRE.TalonSrx.ControlMode.kVoltage); } /* if in voltage mode, update output voltage */ if (_talon.GetControlMode() == CTRE.TalonSrx.ControlMode.kVoltage) { _talon.Set(14.0f * y); } /* copy btns => btnsLast */ System.Array.Copy(_btns, _btnsLast, _btns.Length); }
//reads the talons and updates the data public void updateStatusData() { talonCurrent = (talon.GetOutputCurrent()); talonTemperature = (talon.GetTemperature()); talonVoltage = (talon.GetOutputVoltage()); talonSpeed = (talon.GetSpeed()); talonPosition = (talon.GetPosition()); talonSetpoint = (talon.GetSetpoint()); talonForwardLimitReached = talon.IsFwdLimitSwitchClosed() ? 1 : 0; talonReverseLimitReached = talon.IsRevLimitSwitchClosed() ? 1 : 0; controlMode = talon.GetControlMode(); }
void Loop10Ms() { /* get all the buttons */ FillBtns(ref _btns); /* get the left y stick, invert so forward is positive */ float leftY = kJoystickScaler * _gamepad.GetAxis(1); Deadband(ref leftY); /* debounce the transition from nonzero => zero axis */ float filteredY = leftY; if (filteredY != 0) { /* put in a ramp to prevent the user from flipping their mechanism */ _talon.SetVoltageRampRate(12.0f); /* V per sec */ /* directly control the output */ _talon.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus); _talon.Set(filteredY); } else if (_talon.GetControlMode() == CTRE.TalonSrx.ControlMode.kPercentVbus) { _targetPosition = _talon.GetPosition(); /* user has let go of the stick, lets closed-loop whereever we happen to be */ EnableClosedLoop(); } /* if a button is pressed while stick is let go, servo position */ if (filteredY == 0) { if (_btns[1]) { _targetPosition = _talon.GetPosition(); /* twenty rotations forward */ EnableClosedLoop(); } else if (_btns[4]) { _targetPosition = +10.0f; /* twenty rotations forward */ EnableClosedLoop(); } else if (_btns[2]) { _targetPosition = -10.0f; /* twenty rotations reverese */ EnableClosedLoop(); } } /* copy btns => btnsLast */ System.Array.Copy(_btns, _btnsLast, _btns.Length); }
private static void processInboundData(ArrayList setpointDataList, ArrayList talons) { for (int i = 0; i < setpointDataList.Count; i++) { SetpointData setpointData = (SetpointData)setpointDataList[i]; float setpointVal = (float)(setpointData.getSetpoint()); CTRE.TalonSrx talon = (CTRE.TalonSrx)talons[i]; if (talon.GetControlMode() != setpointData.getMode()) { talon.SetControlMode(setpointData.getMode()); Debug.Print(setpointData.getMode().ToString()); } if (talon.GetSetpoint() != setpointVal) { talon.Set(setpointVal); Debug.Print(setpointVal.ToString()); } } }