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); }
public static void Main() { leftDrive = new CTRE.TalonSrx(1); rightDrive = new CTRE.TalonSrx(2); leftDrive.SetInverted(true); leftDrive.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus); rightDrive.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus); leftDrive.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder); rightDrive.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder); leftDrive.ConfigEncoderCodesPerRev(560); rightDrive.ConfigEncoderCodesPerRev(560); leftDrive.SetSensorDirection(true); leftDrive.SetEncPosition(0); rightDrive.SetEncPosition(0); CTRE.Gamepad gamePad = new CTRE.Gamepad(new CTRE.UsbHostDevice()); /* loop forever */ while (true) { if (gamePad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) { CheesyDrive(gamePad.GetAxis(1), gamePad.GetAxis(2)); CTRE.Watchdog.Feed(); Debug.Print("" + leftDrive.GetPosition() + ":" + rightDrive.GetPosition()); } else { Debug.Print("No Driver Pad"); } /* wait a bit */ System.Threading.Thread.Sleep(10); } }
//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 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.activePoint.isLastPoint ? " 1 \t" : " \t"); _sb.Append(_motionProfileStatus.activePoint.velocityOnly ? " 1 \t" : " \t"); _sb.Append(_motionProfileStatus.activePoint.position); _sb.Append("["); _sb.Append(_motionProfileStatus.activePoint.velocity); _sb.Append("]\t"); _sb.Append("\t\t\t"); _sb.Append(_talon.GetPosition()); _sb.Append("["); _sb.Append(_talon.GetSpeed()); _sb.Append("]"); Debug.Print(_sb.ToString()); } }
public static void Process(CTRE.TalonSrx talon) { /* simple timeout to reduce printed lines */ if (++_instrumLoops1 > 10) { _instrumLoops1 = 0; /* get closed loop info */ float pos = talon.GetPosition(); float spd = talon.GetSpeed(); int err = talon.GetClosedLoopError(); /* 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 */ } } }
public static void Main() { //Set up the motors CTRE.TalonSrx leftmotor = new CTRE.TalonSrx(1); CTRE.TalonSrx rightmotor = new CTRE.TalonSrx(2); CTRE.TalonSrx scoopMotor = new CTRE.TalonSrx(3); CTRE.TalonSrx depthMotor = new CTRE.TalonSrx(4); CTRE.TalonSrx winchMotor = new CTRE.TalonSrx(5); //Set encoders for each motor leftmotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder); rightmotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder); scoopMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder); depthMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder); winchMotor.SetFeedbackDevice(CTRE.TalonSrx.FeedbackDevice.QuadEncoder); //Set direction of the encoders leftmotor.SetSensorDirection(false); rightmotor.SetSensorDirection(false); scoopMotor.SetSensorDirection(false); depthMotor.SetSensorDirection(false); winchMotor.SetSensorDirection(false); //Set Ticks per Rev of the encoders leftmotor.ConfigEncoderCodesPerRev(80); rightmotor.ConfigEncoderCodesPerRev(80); scoopMotor.ConfigEncoderCodesPerRev(80); depthMotor.ConfigEncoderCodesPerRev(50); winchMotor.ConfigEncoderCodesPerRev(80); //Sets PIDF values for each motor// leftmotor.SetP(0, 0.35F); leftmotor.SetI(0, 0.0F); leftmotor.SetD(0, 0.0F); leftmotor.SetF(0, 0.0F); leftmotor.SelectProfileSlot(0); rightmotor.SetP(0, 0.35F); rightmotor.SetI(0, 0.0F); rightmotor.SetD(0, 0.0F); rightmotor.SetF(0, 0.0F); rightmotor.SelectProfileSlot(0); scoopMotor.SetP(0, 0.6F); scoopMotor.SetI(0, 0.0F); scoopMotor.SetD(0, 0.0F); scoopMotor.SetF(0, 0.0F); scoopMotor.SelectProfileSlot(0); depthMotor.SetP(0, 0.6F); depthMotor.SetI(0, 0.0F); depthMotor.SetD(0, 0.0F); depthMotor.SetF(0, 0.0F); depthMotor.SelectProfileSlot(0); winchMotor.SetP(0, 0.6F); winchMotor.SetI(0, 0.0F); winchMotor.SetD(0, 0.0F); winchMotor.SetF(0, 0.0F); winchMotor.SelectProfileSlot(0); ////////////////////////////////// //Sets Nominal Output Voltage for each motor leftmotor.ConfigNominalOutputVoltage(+0.0F, -0.0F); rightmotor.ConfigNominalOutputVoltage(+0.0F, -0.0F); scoopMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F); depthMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F); winchMotor.ConfigNominalOutputVoltage(+0.0F, -0.0F); // Set allowed error for closed loop feedback leftmotor.SetAllowableClosedLoopErr(0, 0); rightmotor.SetAllowableClosedLoopErr(0, 0); scoopMotor.SetAllowableClosedLoopErr(0, 0); depthMotor.SetAllowableClosedLoopErr(0, 0); winchMotor.SetAllowableClosedLoopErr(0, 0); //Set Initial position of the motors leftmotor.SetPosition(0); rightmotor.SetPosition(0); scoopMotor.SetPosition(0); depthMotor.SetPosition(0); winchMotor.SetPosition(0); //Sets Voltage Ramp rate of each motor leftmotor.SetVoltageRampRate(0); rightmotor.SetVoltageRampRate(0); scoopMotor.SetVoltageRampRate(0); depthMotor.SetVoltageRampRate(0); winchMotor.SetVoltageRampRate(0); ArrayList motorSetpointData = new ArrayList(); ArrayList motorStatusData = new ArrayList(); ArrayList talons = new ArrayList(); //Add talons to each motor talons.Add(leftmotor); talons.Add(rightmotor); talons.Add(scoopMotor); talons.Add(depthMotor); talons.Add(winchMotor); //Initializes inbound and outbound message strings to empty String inboundMessageStr = ""; String outboundMessageStr = ""; //Initializes and adds the SetpointData and the StatusData for each motor (ID, mode, setpoint// SetpointData leftmotorSetpointData = new SetpointData(1, 0, 0.0F); SetpointData rightmotorSetpointData = new SetpointData(2, 0, 0.0F); SetpointData scoopMotorSetpointData = new SetpointData(3, 0, 0.0F); SetpointData depthMotorSetpointData = new SetpointData(4, 0, 0.0F); SetpointData winchMotorSetpointData = new SetpointData(5, 0, 0.0F); StatusData leftmotorStatusData = new StatusData(1, leftmotor); StatusData rightmotorStatusData = new StatusData(2, rightmotor); StatusData scoopMotorStatusData = new StatusData(3, scoopMotor); StatusData depthMotorStatusData = new StatusData(4, depthMotor); StatusData winchMotorStatusData = new StatusData(5, winchMotor); motorSetpointData.Add(leftmotorSetpointData); motorSetpointData.Add(rightmotorSetpointData); motorSetpointData.Add(scoopMotorSetpointData); motorSetpointData.Add(depthMotorSetpointData); motorSetpointData.Add(winchMotorSetpointData); motorStatusData.Add(leftmotorStatusData); motorStatusData.Add(rightmotorStatusData); motorStatusData.Add(scoopMotorStatusData); motorStatusData.Add(depthMotorStatusData); motorStatusData.Add(winchMotorStatusData); ////////////////////////////////////////// CTRE.Watchdog.Feed(); //Open up UART communication to the linux box uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200); //uart.DiscardInBuffer(); uart.Open(); //uart.DiscardInBuffer(); //The loop while (true) { //read whatever is available from the UART into the inboundMessageStr motorSetpointData = readUART(ref inboundMessageStr); CTRE.Watchdog.Feed(); if (motorSetpointData.Count > 0) { //if any of the talon positions need to be reset, this will reset them resetEncoderPositions(talons); CTRE.Watchdog.Feed(); //attempt to process whatever was contained in the most recent message processInboundData(motorSetpointData, talons); CTRE.Watchdog.Feed(); //get a bunch of data from the motors in their current states updateMotorStatusData(motorStatusData); CTRE.Watchdog.Feed(); } //package that motor data into a formatted message outboundMessageStr = makeOutboundMessage(motorStatusData); //Debug.Print(outboundMessageStr); CTRE.Watchdog.Feed(); /*string[] teststring = outboundMessageStr.Split(':'); * Debug.Print(" Enc:" + teststring[5] + " Enc2:"+ teststring[14]); * CTRE.Watchdog.Feed();*/ //send that message back to the main CPU //Debug.Print(outboundMessageStr); //if (uart.IsOpen) { Debug.Print("Depth Encoder Value: " + depthMotor.GetPosition()); writeUART(outboundMessageStr); CTRE.Watchdog.Feed(); //} CTRE.Watchdog.Feed(); //Debug.Print(testmotor.GetPosition().ToString()); //CTRE.Watchdog.Feed(); } }