Log in

View Full Version : comparing encoder input against an integer


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);

Mitchell C
02-02-2016, 17:29
Update; i put enc->Get() instead of just enc in the if statements. Now it compiles but when ran the piston affected within the if statement goes crazy and randomly shoots in and out.

ozrien
02-02-2016, 17:38
If you are using quadrature encoders connected to the RIO, you should follow the example code under "Getting Encoder Values".
https://wpilib.screenstepslive.com/s/4485/m/13810/l/241875-encoders-measuring-rotation-of-a-wheel-or-other-shaft

If you are using quad encoders connected to the Talon SRX, follow the examples in the Talon SRX SOftware Reference Manual.

If you are using analog encoder (like MA3) connected to RIO you need to create an analogInput.

But regardless DigitInput is not the correct class. Also in your example enc is a pointer to an object, which has member functions which are typically called to get useful information.