Encoder help

Mitchell_C · February 9, 2016, 6:48pm

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

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

Ether · February 9, 2016, 6:53pm

That’s an analog encoder. You have to plug it into an analog input on the roboRIO. You don’t want to use it for your drivetrain.

Mitchell_C · February 10, 2016, 3:22pm

elaborate please, why wouldn’t you use it for drive train, also it’s kinda all we have available.

Ether · February 10, 2016, 3:30pm

Because it’s an absolute encoder. It measures angles from 0 to 360 degrees.

Mitchell_C · February 10, 2016, 4:07pm

Thank you, this solves a lot of problems.

mmaunu · February 10, 2016, 5:15pm

If you can, I would urge you to do as Ether says and get an incremental encoder. We love these from US Digital:

(Well, we actually use the older S4 line, but they are kind of discontinued [you can order them if you call them up, but they are encouraging new customers to move away from the S4s]. The S4T seems to be the successor to the S4.)

We get them configured as single-ended with bearings. They are not cheap, but they are great!

EDIT: DO NOT USE THESE FOR YOUR DRIVE TO MEASURE DISTANCE. SEE ETHER’S POST BELOW.

If you need to, you can use the MA3 (which are also great encoders…but trickier to use in this scenario). We have used MA3s to maintain state for a shaft that has multiple rotations by creating a wrapper class with an MA3 field. The wrapper class maintains a separate position variable (a double) that gets updated based on a delta we acquire from the MA3. Basically, as the data from the MA3 changes, we calculate the change from the last reading to this reading, and then we update the separate position variable in this class. You should be able to map the value of this separate position variable to distances traveled.

Here’s some (Java-esque) pseudocode.


class MA3Wrapper implements PIDSource {
  MA3 ma3;
  double position, lastMA3Value;

  void updatePosition() {
    double currentMA3Value = ma3.get();
    double delta = currentMA3Value - lastMA3Value;
    //the reality is that the delta calculation is complicated by the wrap-around
    //behavior of the MA3, but you can figure it out...left as an exercise for the reader?

    position += delta;
    lastMA3Value = currentMA3Value;
  }

  public double pidGet() {
    return position;
  }
  
}

With something like this, you would call updatePosition() periodically (in a separate thread or from teleopPeriodic() and autonomousPeriodic()). You could use the wrapper as a PIDSource for a PIDController object. Again, this is Java-flavored but I believe that a C++ implementation would have similar elements.

You will have to map the position variable to some real-world distance to make it meaningful, but you will have to do something similar to give meaning to an incremental encoder’s count as well.

Lastly, if the encoder can turn more than half a revolution between updates, then your position will get off. That might make it a deal breaker in this case depending on what shaft you monitor. It should be fine if you monitor one of your wheel’s shaft.

Hope it was helpful and not just some incomprehensible wall o’ text.

TL;DR It’s doable with an MA3, but it would really be better to get a good incremental encoder.

Ether · February 10, 2016, 6:14pm

*Per the datasheet, max rpm for MA3-A10-250-N is 100 RPM.

mmaunu · February 10, 2016, 7:02pm

We’ll that is incredibly relevant. Edited the post to reflect the fact that this will not work. We did the above to maintain the heading on some swerve modules over the summer, which worked out, but I’m now wondering if we should look at limiting the heading’s rate of change.

As always, thank you.

CHenry18 · February 13, 2016, 1:14pm

Hello,
Can the Talons sr have encoders on them (not the talonsrx). If they can what do I need to buy?