Mitchell C
02-02-2016, 15:58
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);
#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);