Encoder Code

[dboisvert]

Includes the Encoder Header Files

#include "Encoder.h"

Declared the Encoder Variables

	// Declare Encoder Variables for Input
	Encoder *encoderMotor1;
	Encoder *encoderMotor2;

We plugged the into Digital Inputs 3,4 & 5,6

		// Encoders
		encoderMotor1 = new Encoder(3,4);
		encoderMotor2 = new Encoder(5,6);

Created a function in the Initialize Area to start the encoders

	void EncoderInit(void) {
		encoderMotor1.Start();
		encoderMotor2.Start();
	}

Lastly we decided to use the print function to get the encoder values in the debug console

		// Encoder printf
		printf("%d %d\n", encoderMotor1->Get(), encoderMotor2->Get());

We receive errors on the lines encoderMotor1.Start(); & encoderMotor2.Start();

Error: Request for member 'Start' in '((BuiltinDefaultCode*)this)->BuiltinDefaultCode::encoderMotor1', which is of non-class type 'Encoder*'

Error: Request for member 'Start' in '((BuiltinDefaultCode*)this)->BuiltinDefaultCode::encoderMotor2', which is of non-class type 'Encoder*'


Any help would be much appreciated... I probably just need another pair of eyes on the code.

[Alan Anderson]

Originally Posted by dboisvert

		// Encoders
		encoderMotor1 = new Encoder(3,4);
		encoderMotor2 = new Encoder(5,6);

	void EncoderInit(void) {
		encoderMotor1.Start();
		encoderMotor2.Start();
	}

We receive errors on the lines encoderMotor1.Start(); & encoderMotor2.Start();

Your encoderMotorN variables are pointers to Encoder objects. The C++ "dot" syntax is used with objects themselves; use the "->" syntax instead when working with object pointers, like this:

	void EncoderInit(void) {
		encoderMotor1->Start();
		encoderMotor2->Start();
	}

Doesn't the Get() method return a float? The %d format specifier in your printf() is probably going to be an issue.

[dboisvert]

Thanks that fixed the build error.

So what would you suggest for a print format?

Also that didnt get me any feedback from the encoders

[MattD]

Originally Posted by Alan Anderson

Doesn't the Get() method return a float? The %d format specifier in your printf() is probably going to be an issue.

Encoder::Get() returns an int that represents the count, automatically adjusted based on the decoding type (1x, 2x, or 4x). Encoder::GetDistance(), however, returns a double, which is the count scaled by the factor set with Encoder::SetDistancePerPulse().

Originally Posted by dboisvert

Also that didnt get me any feedback from the encoders

Are you calling your EncoderInit() function anywhere?

[dboisvert]

Not that I know of, that is all of our code as of right now.
How do I call it? (again I am a novice C++ user)

#include "WPILib.h"
#include "vxWorks.h" 
#include "Encoder.h"
#include "DigitalInput.h"
#include "Resource.h"
#include "Utility.h"	
#include "WPIStatus.h"
#include "Relay.h"
#include "DigitalModule.h"
#include "stdio.h"
#include <math.h>
#include "Accelerometer.h"
#include "timers.h"

//int delay(int ticks);

class BuiltinDefaultCode : public IterativeRobot
{
	// Declare variable for the robot drive system
	RobotDrive *m_robotDrive;		// robot will use PWM 1-2 for drive motors
	RobotDrive *m_Shooter;			// Robot Shoots with PWM 3-4
	RobotDrive *m_Lift;				// Robot will use PWM 5 & 6 for the Lift Motors
	
	// Declare Encoder Variables for Input
	Encoder *encoderMotor1;
	Encoder *encoderMotor2;
	
	// Declare Accelerometer
	//Accelerometer *accl;
	
	// Declare FlipperDoor Variable
	Relay *flipperdoor; // Flipperdoor
	
	// Declare GearTooth
	DigitalInput *flipperdoorright;
	DigitalInput *flipperdoorleft;
	
	// Declare Gyro
	Gyro* gyro;
	
	// Declare a variable to use to access the driver station object
	DriverStation *m_ds;						// driver station object
	UINT32 m_priorPacketNumber;					// keep track of the most recent packet number from the DS
	UINT8 m_dsPacketsReceivedInCurrentSecond;	// keep track of the ds packets received in the current second
	
	// Declare variables for the two joysticks being used
	Joystick *m_rightStick;			// joystick 1 (arcade stick or right tank stick)
	Joystick *m_leftStick;			// joystick 2 (tank left stick)
	
	static const int NUM_JOYSTICK_BUTTONS = 16;
	bool m_rightStickButtonState[(NUM_JOYSTICK_BUTTONS+1)];
	bool m_leftStickButtonState[(NUM_JOYSTICK_BUTTONS+1)];	
	
	// Declare variables for each of the eight solenoid outputs
	static const int NUM_SOLENOIDS = 8;
	Solenoid *m_solenoids[(NUM_SOLENOIDS+1)];

	enum {							// drive mode selection
		UNINITIALIZED_DRIVE = 0,
		ARCADE_DRIVE = 1,
		TANK_DRIVE = 2
	} m_driveMode;
	
	// Local variables to count the number of periodic loops performed
	UINT32 m_autoPeriodicLoops;
	UINT32 m_disabledPeriodicLoops;
	UINT32 m_telePeriodicLoops;
		
public:
/**
 * Constructor for this "BuiltinDefaultCode" Class.
 * 
 * The constructor creates all of the objects used for the different inputs and outputs of
 * the robot.  Essentially, the constructor defines the input/output mapping for the robot,
 * providing named objects for each of the robot interfaces. 
 */
	
	BuiltinDefaultCode(void)	{
		printf("BuiltinDefaultCode Constructor Started\r");

		// Create a robot using standard right/left robot drive on PWMS 1, 2, 3, and #4
		m_robotDrive = new RobotDrive(1,2);
		m_Shooter = new RobotDrive(3,4);
		m_Lift = new RobotDrive(5,6);
		
		// Encoders
		encoderMotor1 = new Encoder(3,4);
		encoderMotor2 = new Encoder(5,6);
		
		// Accelerometer
		//accl = new Accelerometer(7,8);
		
		// Flipper Door
		flipperdoor = new Relay(1);
		flipperdoorleft = new DigitalInput(1);
		flipperdoorright = new DigitalInput(2);
		
		// Acquire the Driver Station object
		m_ds = DriverStation::GetInstance();
		m_priorPacketNumber = 0;
		m_dsPacketsReceivedInCurrentSecond = 0;

		// Define joysticks being used at USB port #1 and USB port #2 on the Drivers Station
		m_rightStick = new Joystick(1);
		m_leftStick = new Joystick(2);

		// Iterate over all the buttons on each joystick, setting state to false for each
		UINT8 buttonNum = 1;						// start counting buttons at button 1
		for (buttonNum = 1; buttonNum <= NUM_JOYSTICK_BUTTONS; buttonNum++) {
			m_rightStickButtonState[buttonNum] = false;
			m_leftStickButtonState[buttonNum] = false;
		}

		// Iterate over all the solenoids on the robot, constructing each in turn
		UINT8 solenoidNum = 1;						// start counting solenoids at solenoid 1
		for (solenoidNum = 1; solenoidNum <= NUM_SOLENOIDS; solenoidNum++) {
			m_solenoids[solenoidNum] = new Solenoid(solenoidNum);
		}

		// Set drive mode to uninitialized
		m_driveMode = UNINITIALIZED_DRIVE;

		// Initialize counters to record the number of loops completed in autonomous and teleop modes
		m_autoPeriodicLoops = 0;
		m_disabledPeriodicLoops = 0;
		m_telePeriodicLoops = 0;

		printf("BuiltinDefaultCode Constructor Completed\n");
	}
	
	/********************************** Init Routines *************************************/
	
	void RobotInit(void) {
		printf("RobotInit() completed.\r");
	}
	
	void DisabledInit(void) {
		m_disabledPeriodicLoops = 0;			// Reset the loop counter for disabled mode
		ClearSolenoidLEDsKITT();
		// Move the cursor down a few, since we'll move it back up in periodic.
		printf("\x1b[2B");
	}

	void AutonomousInit(void) {
		m_autoPeriodicLoops = 0;				// Reset the loop counter for autonomous mode
		ClearSolenoidLEDsKITT();
	}

	void TeleopInit(void) {
		m_telePeriodicLoops = 0;				// Reset the loop counter for teleop mode
		m_dsPacketsReceivedInCurrentSecond = 0;	// Reset the number of dsPackets in current second
		m_driveMode = UNINITIALIZED_DRIVE;		// Set drive mode to uninitialized
		ClearSolenoidLEDsKITT();
	}
	
	void EncoderInit(void) {
		encoderMotor1.Start();
		encoderMotor2.Start();
	}

	/********************************** Periodic Routines *************************************/
	
	void DisabledPeriodic(void)  {
		static INT32 printSec = (INT32)GetClock() + 1;
		static const INT32 startSec = (INT32)GetClock();

		// feed the user watchdog at every period when disabled
		GetWatchdog().Feed();

		// increment the number of disabled periodic loops completed
		m_disabledPeriodicLoops++;
		
		// while disabled, printout the duration of current disabled mode in seconds
		if (GetClock() > printSec) {
			// Move the cursor back to the previous line and clear it.
			printf("\x1b[1A\x1b[2K");
			printf("Disabled seconds: %d\r\n", printSec - startSec);			
			printSec++;
		}
	}

	void AutonomousPeriodic(void) {
		// feed the user watchdog at every period when in autonomous
		GetWatchdog().Feed();
		
		m_autoPeriodicLoops++;

		// generate KITT-style LED display on the solenoids
		SolenoidLEDsKITT( m_autoPeriodicLoops );
				
		/* the below code (if uncommented) would drive the robot forward at half speed
		 * for two seconds.  This code is provided as an example of how to drive the 
		 * robot in autonomous mode, but is not enabled in the default code in order
		 * to prevent an unsuspecting team from having their robot drive autonomously!
		 */
		/* below code commented out for safety
		if (m_autoPeriodicLoops == 1) {
			// When on the first periodic loop in autonomous mode, start driving forwards at half speed
			m_robotDrive->Drive(0.5, 0.0);			// drive forwards at half speed
		}
		if (m_autoPeriodicLoops == (2 * GetLoopsPerSec())) {
			// After 2 seconds, stop the robot 
			m_robotDrive->Drive(0.0, 0.0);			// stop robot
		}
		*/
	}

	
	void TeleopPeriodic(void) {
		// feed the user watchdog at every period when in autonomous
		GetWatchdog().Feed();
		
		// increment the number of teleop periodic loops completed
		m_telePeriodicLoops++;

		// Encoder printf
		printf("%d %d\n", encoderMotor1->Get(), encoderMotor2->Get());
		
		// Accelerometer printf
		//printf("Accelerometer\n", accl->GetAcceleration());
		
		/*
		 * No longer needed since periodic loops are now synchronized with incoming packets.
		if (m_ds->GetPacketNumber() != m_priorPacketNumber) {
		*/
			/* 
			 * Code placed in here will be called only when a new packet of information
			 * has been received by the Driver Station.  Any code which needs new information
			 * from the DS should go in here
			 */
			 
			m_dsPacketsReceivedInCurrentSecond++;					// increment DS packets received
						
			// put Driver Station-dependent code here

			// Demonstrate the use of the Joystick buttons
			DemonstrateJoystickButtons(m_rightStick, m_rightStickButtonState, "Right Stick", &m_solenoids[1]);
			DemonstrateJoystickButtons(m_leftStick, m_leftStickButtonState, "Left Stick ", &m_solenoids[5]);
		
			// determine if tank or arcade mode, based upon position of "Z" wheel on kit joystick
				m_robotDrive->ArcadeDrive(m_rightStick);			// drive with arcade style (use right stick)
				if (m_driveMode != ARCADE_DRIVE) {
					// if newly entered arcade drive, print out a message
					printf("Arcade Drive\n");
					m_driveMode = ARCADE_DRIVE;
				}
				
			// If you press Button 4 the relay goes forward
			if (m_leftStick->GetRawButton(5) == 1 && flipperdoorright->Get() == 0 ) {
				flipperdoor->Set(Relay::kForward);
				//delay(10);
			} else if (m_leftStick->GetRawButton(4) == 1 && flipperdoorleft->Get() == 0 ){
				flipperdoor->Set(Relay::kReverse);
				//delay(10);
			}
			else{
				flipperdoor->Set(Relay::kOff);
			}
			
			// If you press button 3 the joystick controls the lift instead of the shooter
			if (m_leftStick->GetRawButton(3) == 1) {
				m_Lift->ArcadeDrive(m_leftStick);
			} else {
				m_Shooter->ArcadeDrive(m_leftStick);
			}
			
		/*
		}  // if (m_ds->GetPacketNumber()...
		*/

	} // TeleopPeriodic(void)


/********************************** Continuous Routines *************************************/

	/* 
	 * These routines are not used in this demonstration robot
	 *
	 * 
	void DisabledContinuous(void) {
	}

	void AutonomousContinuous(void)	{
	}

	void TeleopContinuous(void) {
	}
	*/

	
/********************************** Miscellaneous Routines *************************************/
	
	/**
	 * Clear KITT-style LED display on the solenoids
	 * 
	 * Clear the solenoid LEDs used for a KITT-style LED display.
	 */	
	void ClearSolenoidLEDsKITT() {
		// Iterate over all the solenoids on the robot, clearing each in turn
		UINT8 solenoidNum = 1;						// start counting solenoids at solenoid 1
		for (solenoidNum = 1; solenoidNum <= NUM_SOLENOIDS; solenoidNum++) {
			m_solenoids[solenoidNum]->Set(false);
		}
	}
	
	/**
	 * Generate KITT-style LED display on the solenoids
	 * 
	 * This method expects to be called during each periodic loop, with the argument being the 
	 * loop number for the current loop.
	 * 
	 * The goal here is to generate a KITT-style LED display.  (See http://en.wikipedia.org/wiki/KITT )
	 * However, since the solenoid module has two scan bars, we can have ours go in opposite directions!
	 * The scan bar is written to have a period of one second with six different positions.
	 */
	void SolenoidLEDsKITT(UINT32 numloops) {
		unsigned int const NUM_KITT_POSITIONS = 6;
		UINT16 numloop_within_second = numloops % (UINT32)GetLoopsPerSec();

		if (numloop_within_second == 0) {
			// position 1; solenoids 1 and 8 on
			m_solenoids[1]->Set(true);  m_solenoids[8]->Set(true);
			m_solenoids[2]->Set(false); m_solenoids[7]->Set(false);
		} else if (numloop_within_second == ((UINT32)GetLoopsPerSec() / NUM_KITT_POSITIONS)) {
			// position 2; solenoids 2 and 7 on
			m_solenoids[2]->Set(true);  m_solenoids[7]->Set(true);
			m_solenoids[1]->Set(false); m_solenoids[8]->Set(false);
		} else if (numloop_within_second == ((UINT32)GetLoopsPerSec() * 2 / NUM_KITT_POSITIONS)) {
			// position 3; solenoids 3 and 6 on
			m_solenoids[3]->Set(true);  m_solenoids[6]->Set(true);
			m_solenoids[2]->Set(false); m_solenoids[7]->Set(false);
		} else if (numloop_within_second == ((UINT32)GetLoopsPerSec() * 3 / NUM_KITT_POSITIONS)) {
			// position 4; solenoids 4 and 5 on
			m_solenoids[4]->Set(true);  m_solenoids[5]->Set(true);
			m_solenoids[3]->Set(false); m_solenoids[6]->Set(false);
		} else if (numloop_within_second == ((UINT32)GetLoopsPerSec() * 4 / NUM_KITT_POSITIONS)) {
			// position 5; solenoids 3 and 6 on
			m_solenoids[3]->Set(true);  m_solenoids[6]->Set(true);
			m_solenoids[4]->Set(false); m_solenoids[5]->Set(false);
		} else if (numloop_within_second == ((UINT32)GetLoopsPerSec() * 5 / NUM_KITT_POSITIONS)) {
			// position 6; solenoids 2 and 7 on
			m_solenoids[2]->Set(true);  m_solenoids[7]->Set(true);
			m_solenoids[3]->Set(false); m_solenoids[6]->Set(false);
		} 
	}

	/**
	 * Demonstrate handling of joystick buttons
	 * 
	 * This method expects to be called during each periodic loop, providing the following
	 * capabilities:
	 * - Print out a message when a button is initially pressed
	 * - Solenoid LEDs light up according to joystick buttons:
	 *   - When no buttons pressed, clear the solenoid LEDs
	 *   - When only one button is pressed, show the button number (in binary) via the solenoid LEDs
	 *   - When more than one button is pressed, show "15" (in binary) via the solenoid LEDs
	 */
	void DemonstrateJoystickButtons(Joystick *currStick,
									bool *buttonPreviouslyPressed,
									const char *stickString,
									Solenoid *solenoids[]) {
		
		UINT8 buttonNum = 1;				// start counting buttons at button 1
		bool outputGenerated = false;		// flag for whether or not output is generated for a button
		INT8 numOfButtonPressed = 0;		// 0 if no buttons pressed, -1 if multiple buttons pressed
		
		/* Iterate over all the buttons on the joystick, checking to see if each is pressed
		 * If a button is pressed, check to see if it is newly pressed; if so, print out a
		 * message on the console
		 */ 
		for (buttonNum = 1; buttonNum <= NUM_JOYSTICK_BUTTONS; buttonNum++) {
			if (currStick->GetRawButton(buttonNum)) {
				// the current button is pressed, now act accordingly...
				if (!buttonPreviouslyPressed[buttonNum]) {
					// button newly pressed; print out a message
					if (!outputGenerated) {
						// print out a heading if no other button pressed this cycle
						outputGenerated = true;
						printf("%s button pressed:", stickString);
					}
					printf(" %d", buttonNum);
				}
				// remember that this button is pressed for the next iteration
				buttonPreviouslyPressed[buttonNum] = true;
				
				// set numOfButtonPressed appropriately
				if (numOfButtonPressed == 0) {
					// no button pressed yet this time through, set the number correctly
					numOfButtonPressed = buttonNum;
				} else {
					// another button (or buttons) must have already been pressed, set appropriately
					numOfButtonPressed = -1;
				}
			} else {
				buttonPreviouslyPressed[buttonNum] = false;
			}
		}
		
		// after iterating through all the buttons, add a newline to output if needed
		if (outputGenerated) {
			printf("\n");
		}
		
		if (numOfButtonPressed == -1) {
			// multiple buttons were pressed, display as if button 15 was pressed
			DisplayBinaryNumberOnSolenoidLEDs(15, solenoids);
		} else {
			// display the number of the button pressed on the solenoids;
			// note that if no button was pressed (0), the solenoid display will be cleared (set to 0)
			DisplayBinaryNumberOnSolenoidLEDs(numOfButtonPressed, solenoids);
		}
	}
	

	/**
	 * Display a given four-bit value in binary on the given solenoid LEDs
	 */
	void DisplayBinaryNumberOnSolenoidLEDs(UINT8 displayNumber, Solenoid *solenoids[]) {

		if (displayNumber > 15) {
			// if the number to display is larger than can be displayed in 4 LEDs, display 0 instead
			displayNumber = 0;
		}
		
		solenoids[3]->Set( (displayNumber & 1) != 0);
		solenoids[2]->Set( (displayNumber & 2) != 0);
		solenoids[1]->Set( (displayNumber & 4) != 0);
		solenoids[0]->Set( (displayNumber & 8) != 0);
	}
	
};

START_ROBOT_CLASS(BuiltinDefaultCode);

[MattD]

Before you can start reading counts from the encoders, you have to call Start() on them first. It looks like you've defined a function that does this for both of your encoders, but that function is never being called. You should call your EncoderInit() function wherever you'd like to start taking measurements, perhaps in RobotInit() or in your constructor.

void RobotInit()
{
   // .. Existing initialization code..
   EncoderInit();
}

[dboisvert]

That makes a lot more sense and should work. I will post my results tomorrow morning when I have access to the robot.

Thanks a bunch!

[dboisvert]

I was wondering about a few lines of code.

		printf("Encoder Motor 1: %E\n", encoderMotor1->Get());
		printf("Encoder Motor 2: %E\n", encoderMotor2->Get());
		printf("EM1 Distance: %e\n", encoderMotor1->GetDistance());

The first two lines providing the warning

warning: double format, different type arg (arg 2)

I was wondering if this would affect the data at all

Note - I also tried lowercase f instead of capital E

[Jared Russell]

This is because the Encoder::Get() method returns an INT32 (an int), while the %E printf() token indicates a double.

Try:

printf("Encoder Motor 1: %d\n", encoderMotor1->Get());
printf("Encoder Motor 2: %d\n", encoderMotor2->Get());

[dboisvert]

That did the trick. Thanks a bunch!

[krudeboy51]

So where exactly on the DS does the encoder values show up?

[davidalln]

Originally Posted by krudeboy51

So where exactly on the DS does the encoder values show up?

It doesn't automatically. When you printf(), it outputs to the target console. You can either read this using the NetConsole tool (if you've set up your cRIO to send stuff to the NetConsole, the option is in the formatting tool), or by right clicking your connection in Windriver, going to Target Tools -> Target Console (not positive this is the exact option, I don't have Windriver open, but its something similar).