C++ Order Issue

Hi all,
I’m having an issue with a basic C++ concept;
As you can see in the code below, I have switch statements for different modes I’m using for testing.
I want to display the mode when entering it, but not consistently while I’m in the mode. I’m using two variables to make sure it only runs once.

My problem is when I switch modes, it displays the previous mode, not the mode it’s currently switched to. I accidentally spent 1.5 hours on this when my whole goal was motion profiling testing.

#include "WPILib.h"

#include <Drive/DifferentialDrive.h>
#include <IterativeRobot.h>
#include <Joystick.h>
#include <Spark.h>

#include "ctre/Phoenix.h"
#include <iostream>

class Robot: public frc::IterativeRobot {
	WPI_TalonSRX L1 { 1 };
	WPI_TalonSRX L2 { 2 };
	WPI_TalonSRX R1 { 3 };
	WPI_TalonSRX R2 { 4 };

	frc::DifferentialDrive roboDrive { L1, R1 };
	frc::Joystick joy { 0 };

	int m_mode = -1, m_mode_previous = -2;

public:
	void TeleopInit() {
		L1.ConfigMotionAcceleration(500, 10);
		R1.ConfigMotionAcceleration(500, 10);
		L1.ConfigMotionCruiseVelocity(2000, 10);
		R1.ConfigMotionCruiseVelocity(2000, 10);

		L1.ConfigSelectedFeedbackSensor(QuadEncoder, 0, 0);
		R1.ConfigSelectedFeedbackSensor(QuadEncoder, 0, 0);
		L1.SetSelectedSensorPosition(0, 0, 0);
		R1.SetSelectedSensorPosition(0, 0, 0);
		L1.SetSensorPhase(true);
		R1.SetSensorPhase(true);

		//PID
		L1.Config_kF(0, 0, 0);
		L1.Config_kP(0, .4, 0);
		L1.Config_kI(0, 0, 0);
		L1.Config_kD(0, 6, 0);

		R1.Config_kF(0, 0, 0);
		R1.Config_kP(0, .4, 0);
		R1.Config_kI(0, 0, 0);
		R1.Config_kD(0, 6, 0);

		//NOMINAL OUTPUT
		L1.ConfigNominalOutputForward(0, 10);
		L1.ConfigNominalOutputReverse(0, 10);
		R1.ConfigNominalOutputForward(0, 10);
		R1.ConfigNominalOutputReverse(0, 10);

		//Invert
		L1.SetInverted(true);
		L2.SetInverted(true);
		R1.SetInverted(true);
		R2.SetInverted(true);

		//Follow
		L2.Follow(L1);
		R2.Follow(R1);
	}

	void TeleopPeriodic() {
		if (joy.GetRawButton(3)) {
			//X
			m_mode = 1;
		}
		if (joy.GetRawButton(2)) {
			//B
			m_mode = 2;
		}

		if (joy.GetRawButton(1) or joy.GetRawButton(4)) {
			//A & Y
			m_mode = 0;
		}

		switch (m_mode) {
		case 0:
			roboDrive.ArcadeDrive(0, 0);
			break;
		case 1:
			roboDrive.ArcadeDrive(joy.GetRawAxis(1),
					-(joy.GetRawAxis(3) - joy.GetRawAxis(2)));
			break;
		case 2:
			roboDrive.SetSafetyEnabled(false);
			L1.Set(ControlMode::MotionMagic, 1024);
			R1.Set(ControlMode::MotionMagic, 1024);
			break;
		default:
			break;
		}

		std::cout << "
m_mode: " << m_mode << "	m_mode_previous: "
				<< m_mode_previous;

		if (m_mode != m_mode_previous) {
			switch (m_mode) {
			case 0:
				std::cout << "
Mode 0 (Off) selected";
				break;
			case 1:
				std::cout << "
Mode 1 (Drive) selected";
				break;
			case 2:
				std::cout << "
Mode 2 (Motion Magic) selected";
				break;
			case -1:
				std::cout << "
Mode not set";
				break;
			default:
				std::cout << "
Invalid mode selected";
				break;
			}

			m_mode_previous = m_mode;

		}
	}
};

START_ROBOT_CLASS(Robot)

I don’t see anything initially wrong with your code, but you may want to consider the following code and see if that either fixes the issue, or at least is more clear.

I eliminated the m_mode_previous data member - in general the less state the code has, it easier it is to reason with. To keep the desired functionality I created a temporary newMode whose lifetime is only that of the TeleopPeriodic function. Additionally I did the testing and printing before the call to the robot drive functions.

Hope this help,

Steve

	void TeleopPeriodic() {
		auto newMode = m_mode;
		if (joy.GetRawButton(3)) {
			//X
			newMode = 1;
		}
		if (joy.GetRawButton(2)) {
			//B
			newMode = 2;
		}

		if (joy.GetRawButton(1) or joy.GetRawButton(4)) {
			//A & Y
			newMode = 0;
		}

		if( newMode != m_mode) {
			std::cout << "
Switching mode from: " << m_mode << " to: " << newMod;
			m_mode = newMode;

			switch (m_mode) {
			case 0:
				std::cout << "
Mode 0 (Off) selected";
				break;
			case 1:
				std::cout << "
Mode 1 (Drive) selected";
				break;
			case 2:
				std::cout << "
Mode 2 (Motion Magic) selected";
				break;
			case -1:
				std::cout << "
Mode not set";
				break;
			default:
				std::cout << "
Invalid mode selected";
				break;
			}
		}

		switch (m_mode) {
		case 0:
			roboDrive.ArcadeDrive(0, 0);
			break;
		case 1:
			roboDrive.ArcadeDrive(joy.GetRawAxis(1),
					-(joy.GetRawAxis(3) - joy.GetRawAxis(2)));
			break;
		case 2:
			roboDrive.SetSafetyEnabled(false);
			L1.Set(ControlMode::MotionMagic, 1024);
			R1.Set(ControlMode::MotionMagic, 1024);
			break;
		default:
			break;
		}
	}

Same result.

Interesting thing is that when I have it print the variables before the switch statement, it works as expected.

Without the statements, it prints the previous mode.

I’m wondering if it’s some hidden consoleout bug that I’m not familiar with.

The console output by default is line buffered. That means your output may not show up until the next "
" is printed. What happens if you put the "
" at the end of each switch print statement. For extra surety you can use cout::flush().

Thank you so much! I’ve struggled with this issue for about two hours even thought I shouldn’t have been spending the time on this. I faintly remember something about console output needing to be cleared in C++ from a programming class, but from now on I doubt I’ll ever use too many escape keys.

Just as an FYI, you don’t necessarily need to use 2 variables for this, as the Joystick class has a method called GetRawButtonPressed(int) that takes a button number and will only return true once on the rising edge. Likewise, there is a released function for the falling edge. These could simplify your code quite a bit.