void ResetTargetPosition() { _targetPosition = _talon.GetSelectedSensorPosition(0); _talon.SetSelectedSensorPosition(_targetPosition, 0); //Sets current and desired positions to be equal so we don't move unexpectedly at startup Thread.Sleep(100); //wait to make sure SetPosition takes effect }
private bool GoToActuatorPosition(int position) { int distanceToGo = position - leftActuator.GetSelectedSensorPosition(); int direction = distanceToGo > 0 ? 1 : -1; const float magnitude = 1.0f; if (!stopped && (distanceToGo * direction) < 2) { stopped = true; } else if (stopped && (distanceToGo * direction) > 5) { stopped = false; } if (!stopped) { int posDifference = leftActuator.GetSelectedSensorPosition() - rightActuator.GetSelectedSensorPosition(); int sign = posDifference > 0 ? 1 : -1; const float P_Value = .05f; float lSpeed = (direction * magnitude) - (P_Value * posDifference); float rSpeed = (direction * magnitude) + (P_Value * posDifference); leftActuator.Set(ControlMode.PercentOutput, lSpeed); rightActuator.Set(ControlMode.PercentOutput, rSpeed); // Debug.Print(posDifference.ToString()); return(false); } StopAll(); return(true); }
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) { /* directly control the output */ _talon.Set(ControlMode.PercentOutput, filteredY); } else if (_talon.GetControlMode() == ControlMode.PercentOutput) { _targetPosition = _talon.GetSelectedSensorPosition(0); /* 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.GetSelectedSensorPosition(0); /* current position */ EnableClosedLoop(); } else if (_btns[4]) { _targetPosition = +40960.0f; /* ten rotations forward */ EnableClosedLoop(); } else if (_btns[2]) { _targetPosition = -40960.0f; /* ten rotations reverse */ EnableClosedLoop(); } } /* copy btns => btnsLast */ System.Array.Copy(_btns, _btnsLast, _btns.Length); }
private void RunActuators(ref XboxController controller) { //Update Encoders Utils.Print("R, L CURRENT"); Utils.Print(OUTPUT); Utils.Print(RightActuator.GetSelectedSensorPosition()); Utils.Print(""); //Run Motors if (controller.POV == controller.POV_UP) { OUTPUT = 300; } else if (controller.POV == controller.POV_DOWN) { OUTPUT = 800; } //if ((OUTPUT < RightActuator.GetSelectedSensorPosition() && RightActuator.GetSelectedSensorVelocity() > 1) // || (OUTPUT > RightActuator.GetSelectedSensorPosition() && RightActuator.GetSelectedSensorVelocity() < 1)) //{ // LeftActuator.Set(ControlMode.PercentOutput, 0); // RightActuator.Set(ControlMode.PercentOutput, 0); //} //else //{ LeftActuator.Set(ControlMode.MotionMagic, OUTPUT); RightActuator.Set(ControlMode.MotionMagic, OUTPUT); //} }
static float getHoodAngle() { int sensorPos = hood.GetSelectedSensorPosition(); return(linRelate(sensorPos, HOOD_LOWER_BOUND_ANALOG, HOOD_UPPER_BOUND_ANALOG, HOOD_LOWER_BOUND_ANGLE, HOOD_UPPER_BOUND_ANGLE)); }
public double Get_distance() { int l = left_motor.GetSelectedSensorPosition(); int r = right_motor.GetSelectedSensorPosition(); double average = (l + r) * 0.5; //Finds average of what the wheel distance //Finds the distance traveled using the encoders double revolutions = average / Counts; //Finds the counts per revolution double inches = (WheelDiameter * System.Math.PI) * revolutions; //Finds how many inches will be travelled per revolution // double inches_traveled = inches * Drive_Gear_Ratio; //Finds the inches travelled using gear ratio double inches_traveled = inches / Drive_Gear_Ratio; //Finds the inches travelled using gear ratio return(inches_traveled); }
void Instrument() { if (--_timeToColumns <= 0) { _timeToColumns = 400; _sb.Clear(); _sb.Append("topCnt \t"); _sb.Append("btmCnt \t"); _sb.Append("setval \t"); _sb.Append("HasUndr\t"); _sb.Append("IsUnder\t"); _sb.Append(" IsVal \t"); _sb.Append(" IsLast\t"); _sb.Append("VelOnly\t"); _sb.Append(" TargetPos[AndVelocity] \t"); _sb.Append("Pos[AndVelocity]"); Debug.Print(_sb.ToString()); } if (--_timeToPrint <= 0) { _timeToPrint = 40; _sb.Clear(); _sb.Append(_motionProfileStatus.topBufferCnt); _sb.Append("\t\t"); _sb.Append(_motionProfileStatus.btmBufferCnt); _sb.Append("\t\t"); _sb.Append(_motionProfileStatus.outputEnable); _sb.Append("\t\t"); _sb.Append(_motionProfileStatus.hasUnderrun ? " 1 \t" : " \t"); _sb.Append(_motionProfileStatus.isUnderrun ? " 1 \t" : " \t"); _sb.Append(_motionProfileStatus.activePointValid ? " 1 \t" : " \t"); _sb.Append(_motionProfileStatus.isLast ? " 1 \t" : " \t"); _sb.Append(_talon.GetActiveTrajectoryPosition()); _sb.Append("["); _sb.Append(_talon.GetActiveTrajectoryVelocity()); _sb.Append("]\t"); _sb.Append("\t\t\t"); _sb.Append(_talon.GetSelectedSensorPosition(0)); _sb.Append("["); _sb.Append(_talon.GetSelectedSensorVelocity(0)); _sb.Append("]"); Debug.Print(_sb.ToString()); } }
public static void Process(TalonSRX talon) { /* simple timeout to reduce printed lines */ if (++_instrumLoops1 > 10) { _instrumLoops1 = 0; /* get closed loop info */ float pos = talon.GetSelectedSensorPosition(0); float spd = talon.GetSelectedSensorVelocity(0); int err = talon.GetClosedLoopError(0); /* build the string */ _sb.Clear(); _sb.Append(pos); if (_sb.Length < 16) { _sb.Append(' ', 16 - _sb.Length); } _sb.Append(spd); if (_sb.Length < 32) { _sb.Append(' ', 32 - _sb.Length); } _sb.Append(err); if (_sb.Length < 48) { _sb.Append(' ', 48 - _sb.Length); } Debug.Print(_sb.ToString()); /* print data row */ if (++_instrumLoops2 > 8) { _instrumLoops2 = 0; _sb.Clear(); _sb.Append("Position"); if (_sb.Length < 16) { _sb.Append(' ', 16 - _sb.Length); } _sb.Append("Velocity"); if (_sb.Length < 32) { _sb.Append(' ', 32 - _sb.Length); } _sb.Append("Error"); if (_sb.Length < 48) { _sb.Append(' ', 48 - _sb.Length); } Debug.Print(_sb.ToString()); /* print columns */ } } }
static void Initialize() { // Serial port _uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200); _uart.Open(); // Victor SPX Slaves // Left Slave victor1.Set(ControlMode.Follower, 0); // Right Slave victor3.Set(ControlMode.Follower, 2); // Talon SRX Slaves // Feeder Slave feederL.Set(ControlMode.Follower, 0); feederL.SetInverted(true); // Intake Slave intakeLft.Set(ControlMode.Follower, 2); intakeLft.SetInverted(true); // Hood hood.ConfigSelectedFeedbackSensor(FeedbackDevice.Analog, 0, kTimeoutMs); hood.SetSensorPhase(false); hood.Config_kP(0, 30f, kTimeoutMs); /* tweak this first, a little bit of overshoot is okay */ hood.Config_kI(0, 0.0005f, kTimeoutMs); hood.Config_kD(0, 0f, kTimeoutMs); hood.Config_kF(0, 0f, kTimeoutMs); /* use slot0 for closed-looping */ hood.SelectProfileSlot(0, 0); /* set the peak and nominal outputs, 1.0 means full */ hood.ConfigNominalOutputForward(0.0f, kTimeoutMs); hood.ConfigNominalOutputReverse(0.0f, kTimeoutMs); hood.ConfigPeakOutputForward(+1.0f, kTimeoutMs); hood.ConfigPeakOutputReverse(-1.0f, kTimeoutMs); hood.ConfigForwardSoftLimitThreshold(HOOD_LOWER_BOUND_ANALOG, kTimeoutMs); hood.ConfigReverseSoftLimitThreshold(HOOD_UPPER_BOUND_ANALOG, kTimeoutMs); hood.ConfigForwardSoftLimitEnable(true, kTimeoutMs); hood.ConfigReverseSoftLimitEnable(true, kTimeoutMs); /* how much error is allowed? This defaults to 0. */ hood.ConfigAllowableClosedloopError(0, 5, kTimeoutMs); //*********************** // MAY NEED TUNING //*********************** // Turret turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 1, kTimeoutMs); int absPos = turret.GetSelectedSensorPosition(1); turret.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs); turret.SetSelectedSensorPosition(0, 0, kTimeoutMs); turret.SetSensorPhase(true); turret.Config_IntegralZone(20); turret.Config_kP(0, 2f, kTimeoutMs); // tweak this first, a little bit of overshoot is okay turret.Config_kI(0, 0f, kTimeoutMs); turret.Config_kD(0, 0f, kTimeoutMs); turret.Config_kF(0, 0f, kTimeoutMs); // use slot0 for closed-looping turret.SelectProfileSlot(0, 0); // set the peak and nominal outputs, 1.0 means full turret.ConfigNominalOutputForward(0.0f, kTimeoutMs); turret.ConfigNominalOutputReverse(0.0f, kTimeoutMs); turret.ConfigPeakOutputForward(+0.5f, kTimeoutMs); turret.ConfigPeakOutputReverse(-0.5f, kTimeoutMs); turret.ConfigReverseSoftLimitThreshold(TURRET_UPPER_BOUND_ANALOG, kTimeoutMs); turret.ConfigForwardSoftLimitThreshold(TURRET_LOWER_BOUND_ANALOG, kTimeoutMs); turret.ConfigReverseSoftLimitEnable(true, kTimeoutMs); turret.ConfigForwardSoftLimitEnable(true, kTimeoutMs); // how much error is allowed? This defaults to 0. turret.ConfigAllowableClosedloopError(0, 5, kTimeoutMs); // Shooter shooterSensorTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, kTimeoutMs); shooterSensorTalon.SetSensorPhase(false); shooterSensorTalon.ConfigPeakOutputReverse(0.0f, kTimeoutMs); shooterSensorTalon.ConfigPeakOutputForward(1.0f, kTimeoutMs); shooterVESC = new PWMSpeedController(CTRE.HERO.IO.Port3.PWM_Pin7); shooterVESC.Set(0); pcm.SetSolenoidOutput(shooterFeedbackLEDPort, false); }