| Mitchell C |
02-02-2016 03:58 PM |
comparing encoder input against an integer
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.
Quote:
Code:
#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);
|
|
