I’m trying to get code to work to test if our encoders are properly working, we are using absolute encoders and talons, we (think) we know that the wiring is correct.
#include "WPILib.h" #include "Commands/Command.h" #include "Commands/ExampleCommand.h" #include "CommandBase.h" #include "NetworkTables/NetworkTable.h" class Robot: public IterativeRobot { public: Victor *V1, *V2, *V3, *V4, *V5, *V6, *V7, *V8; Joystick *leftJoy, *rightJoy, *controller; NetworkTable *table; DoubleSolenoid *MPS1, *MPS2, *MPS3, *MPS4; Compressor *compressor; CANTalon *T1, *T2, *T3; Timer *timer; DigitalInput *enc; //Encoder *encoder; //Encoder *sampleEncoder = new Encoder(0, 1, false, Encoder::EncodingType::k4X); Robot() { //drive train motors //left back wheel V1 = new Victor(0); //red V2 = new Victor(1); //green //left front wheel V3 = new Victor(2); //blue V4 = new Victor(3); //yellow //right rear wheel V5 = new Victor(4); //black V6 = new Victor(5); //white //right front wheel V7 = new Victor(6); //blue-red V8 = new Victor(7); //black-red //driver and operator controls leftJoy = new Joystick(2); rightJoy = new Joystick(1); controller = new Joystick(0); //Talons T1 = new CANTalon(0); T2 = new CANTalon(1); T3 = new CANTalon(2); //compressor compressor = new Compressor(0); //pneumatics MPS1 = new DoubleSolenoid(0, 1); MPS2 = new DoubleSolenoid(2, 3); MPS3 = new DoubleSolenoid(4, 5); MPS4 = new DoubleSolenoid(6, 7); //network tables table = NetworkTable::GetTable("SmartDashboard"); //timers timer = new Timer(); enc = new DigitalInput(0); /* //encoder sampleEncoder = new Encoder(0, 1, false, Encoder::EncodingType::k4X); sampleEncoder->SetMaxPeriod(.1); sampleEncoder->SetMinRate(10); sampleEncoder->SetDistancePerPulse(5); sampleEncoder->SetReverseDirection(true); sampleEncoder->SetSamplesToAverage(7); */ } void RobotInit() override { CameraServer::GetInstance()->SetQuality(50); //the camera name (ex "cam0") can be found through the roborio web interface CameraServer::GetInstance()->StartAutomaticCapture("cam0"); SmartDashboard::init(); compressor->SetClosedLoopControl(true); } void OperatorControl() { while (IsOperatorControl() && IsEnabled()) { /** robot code here! **/ Wait(0.005); // wait for a motor update time } } void AutonomousInit() { //sampleEncoder->Reset(); T1->Set(-1); T2->Set(-1); Wait(3); T1->Set(0); T2->Set(0); V1->Set(1); V2->Set(1); V3->Set(1); Wait(4); V1->Set(0); V2->Set(0); V3->Set(0); /* MPS1->Set(DoubleSolenoid::kForward); MPS2->Set(DoubleSolenoid::kForward); MPS3->Set(DoubleSolenoid::kForward); MPS4->Set(DoubleSolenoid::kForward); Wait(3); MPS1->Set(DoubleSolenoid::kReverse); MPS2->Set(DoubleSolenoid::kReverse); MPS3->Set(DoubleSolenoid::kOff); MPS4->Set(DoubleSolenoid::kOff); */ } void TeleopInit() { } void TeleopPeriodic() { //DRIVER CONTROLS if (leftJoy->GetRawButton(5) || rightJoy->GetRawButton(10)) //Buttons 6 and 11 on joysticks { TankDrive(-leftJoy->GetRawAxis(1) * 0.5, -rightJoy->GetRawAxis(1) * 0.5); } TankDrive(-leftJoy->GetRawAxis(1), -rightJoy->GetRawAxis(1)); /* if (controller->GetRawAxis(1) > 0.25) //Left Thumbstick on controller { MPS4->Set(DoubleSolenoid::kForward); } else if (controller->GetRawAxis(1) < -0.25) { MPS4->Set(DoubleSolenoid::kReverse); } else { MPS4->Set(DoubleSolenoid::kOff); } */ if (controller->GetRawAxis(5) > 0.25) { MPS3->Set(DoubleSolenoid::kForward); } else if(controller->GetRawAxis(5) < -0.25) { MPS3->Set(DoubleSolenoid::kReverse); } else { MPS3->Set(DoubleSolenoid::kOff); } if (controller->GetRawButton(1)) //A Button on controller { MPS2->Set(DoubleSolenoid::kForward); } else { MPS2->Set(DoubleSolenoid::kReverse); } if(controller->GetRawButton(4)) { MPS1->Set(DoubleSolenoid::kForward); } else { MPS1->Set(DoubleSolenoid::kReverse); } if(controller->GetRawButton(5)) { V1->Set(1); V2->Set(1); V3->Set(1); Wait(2); T1->Set(1); T2->Set(1); V4->Set(1); V5->Set(1); Wait(.5); V1->Set(0); V2->Set(0); V3->Set(0); Wait(2.5); T1->Set(0); T2->Set(0); V4->Set(-1); V5->Set(-1); Wait(2); V4->Set(0); V5->Set(0); } //ENCODER TEST STUFF if(enc < 10) { MPS2->Set(DoubleSolenoid::kForward); } else if(enc > 10) { MPS1->Set(DoubleSolenoid::kForward); } } void DisabledPeriodic() { } void TestInit() { V1->Set(1); Wait(3.0); V1->Set(0); V2->Set(1); Wait(3.0); V2->Set(0); V3->Set(1); Wait(3); V3->Set(0); V4->Set(1); Wait(3); V4->Set(0); V5->Set(1); Wait(3); V5->Set(0); V6->Set(1); Wait(3); V6->Set(0); V7->Set(1); Wait(3); V7->Set(0); V8->Set(1); Wait(3); V8->Set(0); T1->Set(1); Wait(3); T1->Set(0); T2->Set(1); Wait(3); T2->Set(0); } void TankDrive(double left, double right) { V4->Set(left); V3->Set(left); V1->Set(left); V2->Set(left); V7->Set(-right); V8->Set(-right); V5->Set(-right); V6->Set(-right); T1->Set(left); T2->Set(-right); T3->Set(left); //SmartDashboard::PutNumber("Left Drivetrain", left); //SmartDashboard::PutNumber("Right Drivetrain", -right); } }; START_ROBOT_CLASS(Robot);