public static void Main() { /* create a gamepad object */ CTRE.Gamepad myGamepad = new CTRE.Gamepad(new CTRE.UsbHostDevice()); /* create a talon, the Talon Device ID in HERO LifeBoat is zero */ CTRE.TalonSrx myTalon = new CTRE.TalonSrx(0); /* simple counter to print and watch using the debugger */ int counter = 0; /* loop forever */ while (true) { /* added inside the while loop */ if (myGamepad.GetConnectionStatus() == CTRE.UsbDeviceConnection.Connected) { /* print the axis value */ Debug.Print("axis:" + myGamepad.GetAxis(1)); /* pass axis value to talon */ myTalon.Set(myGamepad.GetAxis(1)); /* allow motor control */ CTRE.Watchdog.Feed(); } /* increment counter */ ++counter; /* try to land a breakpoint here */ /* wait a bit */ System.Threading.Thread.Sleep(10); } }
public BallLaunchModule() : base("BALL LAUNCH") { Winch = new CTRE.TalonSrx(WINCH_TALONSRX_ID); Dog = new CTRE.TalonSrx(DOG_TALONSRX_ID); State = BallLaunchState.Unknown; Mode = FireMode.Normal; }
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); } }
//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()); } } }
public DriveModule() : base("DRIVE") { /* Create our device objects */ Right = new CTRE.TalonSrx(RIGHT_TALONSRX_ID); RightSlave = new CTRE.TalonSrx(RIGHT_SLAVE_TALONSRX_ID); Left = new CTRE.TalonSrx(LEFT_TALONSRX_ID); LeftSlave = new CTRE.TalonSrx(LEFT_SLAVE_TALONSRX_ID); }
/// <summary> /// Create a Pigeon object that communciates with Pigeon through the Gadgeteer ribbon /// cable connected to a Talon on CAN Bus. /// </summary> /// <param name="talonSrx"> Talon SRX object that Pigeon is connected to.</param> public PigeonImu(CTRE.TalonSrx talonSrx) { _deviceId = (UInt32)0x02000000 | talonSrx.GetDeviceNumber(); CTRE.Native.CAN.Send(CONTROL_1 | _deviceId, 0x00000000, 0, 100); _ParamContainer = new ParamContainer(_deviceId, PARAM_REQUEST, PARAM_RESPONSE, PARAM_SET); }
//Password to module wifi is "password1" /** entry point of the application */ public static void Main() { ds = new CTRE.FRC.DriverStation(wifiport); //Create new Driver station object at port 4 _gamepad = new CTRE.Controller.GameController(ds, 0); //Set controller to look at DS with ID 0 //Create two TalonSrx objects and set their control mode left1 = new CTRE.TalonSrx(1); right1 = new CTRE.TalonSrx(2); left1.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus); right1.SetControlMode(CTRE.TalonSrx.ControlMode.kPercentVbus); //Set IP module looks for and is configured with, Computer must set to Static IP ds.SendIP(new byte[] { 10, 0, 33, 2 }, new byte[] { 10, 0, 33, 5 }); while (true) { //Must be called for DS to function ds.update(); //Send Battery Voltage information to Driver Station ds.SendBattery(left1.GetBusVoltage()); if (ds.IsAuton() && ds.IsEnabled()) { //Auton Code while enabled } else if (ds.IsAuton() && !ds.IsEnabled()) { //Auton Code while disabled } else if (!ds.IsAuton() && ds.IsEnabled()) { //Teleop Code while enabled Drive(); Debug.Print(stringBuilder.ToString()); ds.SendUDP(2550, Encoding.UTF8.GetBytes(stringBuilder.ToString())); stringBuilder.Clear(); } else if (!ds.IsAuton() && !ds.IsEnabled()) { //Teleop Code while disabled } } }
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()); } } }
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 void RunForever() { int toPrint = 0; int loops = 0; CTRE.TalonSrx _talon = new CTRE.TalonSrx(0); /* make a talon with deviceId 0 */ CTRE.PigeonImu _pigeon; /** Use a USB gamepad plugged into the HERO */ CTRE.Gamepad _gamepad = new CTRE.Gamepad(CTRE.UsbHostDevice.GetInstance()); CTRE.GamepadValues _gamepadValues = new CTRE.GamepadValues(); uint _oldBtns = 0; StringBuilder _sb = new StringBuilder(); /** choose pigeon connected to Talon or on CAN Bus */ if (true) { _pigeon = new CTRE.PigeonImu(0); /* Pigeon is on CAN Bus with device ID 0 */ } else { _pigeon = new CTRE.PigeonImu(_talon); /* Pigeon is connected to _talon via Gadgeteer cable.*/ } /* set frame rates */ _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.BiasedStatus_4_Mag, 17); _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_1_General, 217); _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_11_GyroAccum, 9999); _pigeon.SetStatusFrameRateMs(CTRE.PigeonImu.StatusFrameRate.CondStatus_9_SixDeg_YPR, 0); _pigeon.SetAccumZAngle(217); /* loop forever */ while (true) { int ret = 0; /* signals for motion */ float[] ypr = new float[3]; float[] wxyz = new float[4]; float compassHeading, magnitudeMicroTeslas; float tempC; CTRE.PigeonImu.PigeonState state; Int16[] rmxyz = new Int16[3]; Int16[] bmxyz = new Int16[3]; Int16[] baxyz = new short[3]; Int16 baNorm; float[] xyz_dps = new float[3]; float [] tilts_XZ_YZ_XY = new float[3]; UInt32 timeSec; float fusedHeading; CTRE.PigeonImu.FusionStatus fusionStatus = new CTRE.PigeonImu.FusionStatus(); CTRE.PigeonImu.GeneralStatus generalStatus = new CTRE.PigeonImu.GeneralStatus(); /* get sigs */ ret |= _pigeon.GetYawPitchRoll(ypr); ret |= _pigeon.Get6dQuaternion(wxyz); compassHeading = _pigeon.GetCompassHeading(); ret |= _pigeon.GetLastError(); magnitudeMicroTeslas = _pigeon.GetCompassFieldStrength(); ret |= _pigeon.GetLastError(); tempC = _pigeon.GetTemp(); ret |= _pigeon.GetLastError(); state = _pigeon.GetState(); ret |= _pigeon.GetLastError(); ret |= _pigeon.GetRawMagnetometer(rmxyz); ret |= _pigeon.GetBiasedMagnetometer(bmxyz); ret |= _pigeon.GetBiasedAccelerometer(baxyz, out baNorm); ret |= _pigeon.GetRawGyro(xyz_dps); ret |= _pigeon.GetAccelerometerAngles(tilts_XZ_YZ_XY); timeSec = _pigeon.GetUpTime(); fusedHeading = _pigeon.GetFusedHeading(fusionStatus); ret |= _pigeon.GetGeneralStatus(generalStatus); /* get buttons */ _gamepad.GetAllValues(ref _gamepadValues); if (_gamepadValues.pov == 0) // up { _pigeon.EnableTemperatureCompensation(true); } if (_gamepadValues.pov == 4) // dn { _pigeon.EnableTemperatureCompensation(false); } if (_oldBtns != _gamepadValues.btns) { int btnIdx = GetButtonFromBits(_gamepadValues.btns); switch (btnIdx) { case 4: if (toPrint < kToPrintMaxInclus) { ++toPrint; } break; case 2: if (toPrint > kToPrintMin) { --toPrint; } break; case 5: // top left _pigeon.SetYaw(100); _pigeon.SetFusedHeading(100); break; case 7: // btm left break; case 6: // top right _pigeon.SetYawToCompass(); _pigeon.SetFusedHeadingToCompass(); break; case 8: // btm right _pigeon.AddYaw(45); _pigeon.AddYaw(45); break; case 10: // enter mag cal start btn _pigeon.EnterCalibrationMode(CTRE.PigeonImu.CalibrationMode.Temperature); break; case 9: // accel 0,0, _pigeon.EnterCalibrationMode(CTRE.PigeonImu.CalibrationMode.Temperature); break; } _oldBtns = _gamepadValues.btns; } if (++loops >= 20) { loops = 0; /* build row */ switch (toPrint) { case 0: _sb.Append("YPR:"); _sb.Append(ToString(ypr)); break; case 1: _sb.Append("FusedHeading:"); _sb.Append(fusedHeading); _sb.Append(" deg, "); _sb.Append(fusionStatus.description); break; case 2: _sb.Append("Compass:"******" deg, "); _sb.Append(magnitudeMicroTeslas); _sb.Append("uT"); break; case 3: _sb.Append("tempC:"); _sb.Append(tempC); _sb.AppendLine(); break; case 4: _sb.Append("PigeonState:"); _sb.Append(state); _sb.AppendLine(); break; case 5: _sb.Append("BiasedM:"); _sb.Append(ToString(bmxyz)); break; case 6: _sb.Append("RawM:"); _sb.Append(ToString(rmxyz)); break; case 7: _sb.Append("BiasedAccel:"); _sb.Append(ToString(baxyz)); break; case 8: _sb.Append("GyroDps:"); _sb.Append(ToString(xyz_dps)); break; case 9: _sb.Append("AccelTilts:"); _sb.Append(ToString(tilts_XZ_YZ_XY)); break; case 10: _sb.Append("Quat:"); _sb.Append(ToString(wxyz)); break; case 11: _sb.Append("UpTime:"); _sb.Append(timeSec); break; case 12: _sb.Append("GS:"); _sb.Append(generalStatus.description); break; case 13: _sb.Append("tempCompensationCount:"); _sb.Append(generalStatus.tempCompensationCount); _sb.Append(" tempC:"); _sb.Append(generalStatus.tempC); _sb.Append(" noMotionBiasCount:"); _sb.Append(generalStatus.noMotionBiasCount); break; } kToPrintMin = 0; kToPrintMaxInclus = 13; //_sb.Append(yaw); _sb.Append(","); _sb.Append(pitch); _sb.Append(","); _sb.Append(roll); _sb.AppendLine(); //_sb.Append(w); _sb.Append(","); _sb.Append(x); _sb.Append(","); _sb.Append(y); _sb.Append(","); _sb.Append(z); _sb.AppendLine(); //_sb.Append(compassHeading); _sb.Append(","); _sb.Append(magnitudeMicroTeslas); _sb.AppendLine(); //_sb.Append(tempC); _sb.AppendLine(); //_sb.Append(state); _sb.AppendLine(); //_sb.Append(rmx); _sb.Append(","); _sb.Append(rmy); _sb.Append(","); _sb.Append(rmz); _sb.AppendLine(); //_sb.Append(bmx); _sb.Append(","); _sb.Append(bmy); _sb.Append(","); _sb.Append(bmz); _sb.AppendLine(); //_sb.Append(bam); _sb.Append(","); _sb.Append(bay); _sb.Append(","); _sb.Append(baz); _sb.Append(","); _sb.Append(baNorm); _sb.AppendLine(); //_sb.Append(x_dps); _sb.Append(","); _sb.Append(y_dps); _sb.Append(","); _sb.Append(z_dps); _sb.AppendLine(); //_sb.Append(tiltXZ); _sb.Append(","); _sb.Append(tiltYZ); _sb.Append(","); _sb.Append(tiltXY); _sb.AppendLine(); //_sb.Append(timeSec); _sb.Append(","); //_sb.Append(fusedHeading); _sb.Append(","); _sb.Append(bIsValid); _sb.Append(","); _sb.Append(bIsFusing); _sb.AppendLine(); //_sb.Append(ret); _sb.Append(","); } /* print line and clear buffer */ if (_sb.Length != 0) { Debug.Print(_sb.ToString()); _sb.Clear(); } /* 10ms loop */ Thread.Sleep(10); if (_pigeon.HasResetOccured()) { int resetCount; int resetFlags; int firmVers; resetCount = _pigeon.GetResetCount(); resetFlags = _pigeon.GetResetFlags(); firmVers = _pigeon.GetFirmVers(); Debug.Print("Pigeon Reset detected..."); Debug.Print(" Pigeon Reset Count:" + resetCount.ToString("D")); Debug.Print(" Pigeon Reset Flags:" + resetFlags.ToString("X")); Debug.Print(" Pigeon FirmVers:" + firmVers.ToString("X")); Debug.Print(" Waiting for delay"); Thread.Sleep(3000); } } }
public StatusData(int talonDeviceID, CTRE.TalonSrx talon) { this.talonDeviceID = talonDeviceID; this.talon = talon; }
public static void Main() { 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); 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); leftMotor.SetSensorDirection(false); rightMotor.SetSensorDirection(true); scoopMotor.SetSensorDirection(false); depthMotor.SetSensorDirection(false); winchMotor.SetSensorDirection(false); leftMotor.ConfigEncoderCodesPerRev(80); rightMotor.ConfigEncoderCodesPerRev(80); scoopMotor.ConfigEncoderCodesPerRev(80); depthMotor.ConfigEncoderCodesPerRev(80); winchMotor.ConfigEncoderCodesPerRev(80); 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); 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); leftMotor.SetAllowableClosedLoopErr(0, 0); rightMotor.SetAllowableClosedLoopErr(0, 0); scoopMotor.SetAllowableClosedLoopErr(0, 0); depthMotor.SetAllowableClosedLoopErr(0, 0); winchMotor.SetAllowableClosedLoopErr(0, 0); leftMotor.SetPosition(0); rightMotor.SetPosition(0); scoopMotor.SetPosition(0); depthMotor.SetPosition(0); winchMotor.SetPosition(0); 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(); talons.Add(leftMotor); talons.Add(rightMotor); talons.Add(scoopMotor); talons.Add(depthMotor); talons.Add(winchMotor); String inboundMessageStr = ""; String outboundMessageStr = ""; 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(); uart = new System.IO.Ports.SerialPort(CTRE.HERO.IO.Port1.UART, 115200); uart.Open(); while (true) { //read whatever is available from the UART into the inboundMessageStr motorSetpointData = readUART(ref inboundMessageStr); CTRE.Watchdog.Feed(); //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); CTRE.Watchdog.Feed(); //send that message back to the main CPU writeUART(outboundMessageStr); //Debug.Print("set=" + winchMotor.GetSetpoint().ToString() + " mod=" + winchMotor.GetControlMode().ToString() + // "pos=" + winchMotor.GetPosition().ToString() + " vel=" + winchMotor.GetSpeed().ToString() + // "err=" + winchMotor.GetClosedLoopError().ToString() + "vlt=" + winchMotor.GetOutputVoltage()); //Debug.Print(DateTime.Now.ToString()); CTRE.Watchdog.Feed(); //keep the loop timing consistent //TODO: evaluate if this is necessary //System.Threading.Thread.Sleep(10); } }
public static void Main() { //Set up the motors CTRE.TalonSrx motor = 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 motor.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 motor.SetSensorDirection(false); /* rightMotor.SetSensorDirection(true); * scoopMotor.SetSensorDirection(false); * depthMotor.SetSensorDirection(false); * winchMotor.SetSensorDirection(false); */ //Set Ticks per Rev of the encoders motor.ConfigEncoderCodesPerRev(80); /* rightMotor.ConfigEncoderCodesPerRev(80); * scoopMotor.ConfigEncoderCodesPerRev(80); * depthMotor.ConfigEncoderCodesPerRev(80); * winchMotor.ConfigEncoderCodesPerRev(80); */ //Sets PIDF values for each motor// motor.SetP(0, 0.35F); motor.SetI(0, 0.0F); motor.SetD(0, 0.0F); motor.SetF(0, 0.0F); motor.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 motor.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 motor.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 motor.SetPosition(0); /* rightMotor.SetPosition(0); * scoopMotor.SetPosition(0); * depthMotor.SetPosition(0); * winchMotor.SetPosition(0); */ //Sets Voltage Ramp rate of each motor motor.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(motor); /*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 motor = 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 motor = new StatusData(1, motor); /*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(motor); /*motorSetpointData.Add(rightMotorSetpointData); * motorSetpointData.Add(scoopMotorSetpointData); * motorSetpointData.Add(depthMotorSetpointData); * motorSetpointData.Add(winchMotorSetpointData); */ motorStatusData.Add(motor); /*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.Open(); //The loop while (true) { //read whatever is available from the UART into the inboundMessageStr motorSetpointData = readUART(ref inboundMessageStr); CTRE.Watchdog.Feed(); //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); CTRE.Watchdog.Feed(); //send that message back to the main CPU writeUART(outboundMessageStr); CTRE.Watchdog.Feed(); } }
public Battery(CTRE.TalonSrx talon) { _talon = talon; }