We are trying to do encoders for the first time as a team and are having a lot of trouble. We are using C++ and absolute encoders(
http://www.andymark.com/product-p/am-2899.htm). We are not sure how do do the code for them and haven't been able to find anything useful anywhere we look. Any help would be apreciated
Code:
#include "WPILib.h"
#include "Joystick.h"
#include "Commands/Command.h"
#include "NetworkTables/NetworkTable.h"
#include <sstream>
#include <string>
//#include "MyRobot.h"
//#include "MA3Encoder.h"
class Robot: public IterativeRobot
{
public:
Victor *V1, *V2, *V3, *V4, *V5, *V6, *V7, *V8;
Joystick *leftJoy, *rightJoy, *controller;
//SmartDashboard *dash;
DoubleSolenoid *MPS1, *MPS2, *MPS3, *MPS4;
Compressor *compressor;
CANTalon *T1, *T2, *T3;
Timer *timer;
//AnalogInput *enc;
Encoder *leftEncoder, *rightEncoder;
//Encoder *encoder;
//Encoder *sampleEncoder = new Encoder(0, 1, false, Encoder::EncodingType::k4X);
const int E_L_ACHANNEL = 3;
const int E_L_BCHANNEL = 4;
const int E_L_DISTPERPULSE = 1;
const double E_L_MAXPERIOD = 1.0;
const bool E_L_REVERSE = false;
//right encoder
const int E_R_ACHANNEL = 1;
const int E_R_BCHANNEL = 2;
const int E_R_DISTPERPULSE = 1;
const double E_R_MAXPERIOD = 1.0;
const bool E_R_REVERSE = true;
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");
//dash = new SmartDashboard();
//timers
timer = new Timer();
rightEncoder = new Encoder(8,1,false, Encoder::EncodingType::k4X);
//enc = new AnalogInput(0);
//SmartDashboard.PutNumber("Encoder", E_L_ACHANNEL);
leftEncoder = new Encoder(E_L_ACHANNEL, E_L_BCHANNEL, E_L_REVERSE);
leftEncoder->SetDistancePerPulse(E_L_DISTPERPULSE);
leftEncoder->SetMaxPeriod(E_L_MAXPERIOD);
//rightEncoder = new Encoder(E_R_ACHANNEL, E_R_BCHANNEL, E_R_REVERSE);
//rightEncoder->SetDistancePerPulse(E_R_DISTPERPULSE);
//rightEncoder->SetMaxPeriod(E_R_MAXPERIOD);
}
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()
{
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() {leftEncoder->Reset(); //rightEncoder->Reset();
}
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);
}
while(controller->GetRawButton(3))
{
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);
}
//TEST STUFF
auto str = std::to_string(rightEncoder->Get());
SmartDashboard::PutString("DB/String 0","my 21 char it's a bhatt");
SmartDashboard::PutString("DB/String 1",str);
//SmartDashboard::PutNumber("DB/String 2",2.1);
//SmartDashboard::PutNumber("DB/String 5", 1.2);
if(leftEncoder->GetRaw() > 1)
{
MPS1->Set(DoubleSolenoid::kForward);
}
else if(leftEncoder->GetRaw() < 1 && leftEncoder->GetRaw() > .000001)
{
MPS2->Set(DoubleSolenoid::kForward);
}
else if(leftEncoder->GetRaw() == 1)
{
V1->Set(1);
Wait(3);
V1->Set(0);
}
else if(leftEncoder->GetRaw() == 0)
{
V1->Set(1);
}
}
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);