Limit switches in autonomous

Technical C/C++
1

Our limit switches [to regulate Jaguar-implemented ‘elevator’ for arm] are working fine in teleop but not in autonomous. I’ve tried putting a print to driver station in the limit-switch statement in autonomous, and it never prints:

// test if limit switch works: it doesn't. 
	  
	  if (topElevatorLimitSwitch->Get() == false){
	  elevatorJaguar->Set(0.0);
	 driverStationLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Elevator stopped"); 
	driverStationLCD->UpdateLCD();
}
	else {
	     elevatorJaguar->Set(1.0); // Run elevator up
	    driverStationLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Elevator up"); 
	    driverStationLCD->UpdateLCD();
					}

I tried running just that bit during autonomous, and it never runs: the motor does nothing, the lights indicate it’s not running, and the driver station never displays either printf.

Here’s the full autonomous code:


LencVal = lEncoder->Get();
		RencVal = rEncoder->Get();
		
		//it turns out, the input must be less than ~21 characters.  Anything longer won't be displayed
		driverStationLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Lenc cnt:%d         ", LencVal); 
		driverStationLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Renc cnt:%d         ", RencVal); 
		driverStationLCD->UpdateLCD();
				

		//compressor control
		int compressorValue = pneumaticLimitSwitch->Get();
		if (compressorValue == 1) {
			compressorRelay->Set(Relay::kOff);  // If relay is true (120 PSI) turn off compressor
		}
		else {
			compressorRelay->Set(Relay::kOn);
		}
		
		leftValue = lLineTracker->Get();	// read the line tracking sensors
		middleValue = mLineTracker->Get();
		rightValue = rLineTracker->Get();

if ( (LencVal < 7300 )){  // overshoots: goal encoder count is 7390
			
			robotDrive->TankDrive(-0.25, 0.25);
			driverStationLCD->Printf(DriverStationLCD::kUser_Line2, 1, "Still driving"); 
			driverStationLCD->UpdateLCD();
			if (topElevatorLimitSwitch->Get() == false){
				elevatorJaguar->Set(0.0);
				driverStationLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Elevator stopped"); 
				driverStationLCD->UpdateLCD();
			}
			else {
				elevatorJaguar->Set(1.0); // Run elevator up
				driverStationLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Elevator up"); 
				driverStationLCD->UpdateLCD();
			}
						

		
		
			
		}
		
		else {
			robotDrive->TankDrive(0.0, 0.0);
			elevatorJaguar->Set(0.0);
			gripperSol->Set(true); // If we're at the T, let's score
			driverStationLCD->Printf(DriverStationLCD::kUser_Line2, 1, "At T"); 
			driverStationLCD->UpdateLCD();
			
		}

The Jaguar happily obeys the commands to stop when the encoder value is reached.

Teleop limit switch stuff works perfectly.


//compressor control
		int compressorValue = pneumaticLimitSwitch->Get();
		if (compressorValue == 1) {
			compressorRelay->Set(Relay::kOff);  // If relay is true (120 PSI) turn off compressor
		}
		else {
			compressorRelay->Set(Relay::kOn);
		}
			
		
		
		// move the elevator up, as long as the limit switch hasn't been pressed
		if ((elevatorJS->GetY() < 0.0) && (topElevatorLimitSwitch->Get() == true)) { // false means pressed...
			elevatorJaguar->Set(elevatorJS->GetY()); 
		}
		
		// move the elevator down, as long as the limit switch hasn't been pressed
		else if (elevatorJS->GetY() > 0.0 && bottomElevatorLimitSwitch->Get() == true){
			elevatorJaguar->Set(elevatorJS->GetY());
		}
		
		// in all other cases, turn motor off
		else {
			elevatorJaguar->Set(0.0);
		}
		
	
		// y = x linear relationship
		//robotDrive->TankDrive(rJS->GetY(),-lJS->GetY()); // inverted with - to circumvent non-con wiring
		

		// Joystick sensitivity adjustment: y = .6x^3 + .4x, leftJS negative (see above)
		//driverStationLCD->Printf(DriverStationLCD::kUser_Line2, 1, "L:%f         ", lJS->GetY()); 
		//driverStationLCD->Printf(DriverStationLCD::kUser_Line3, 1, "R:%f         ", rJS->GetY()); 
		//driverStationLCD->UpdateLCD();
				
		robotDrive->TankDrive(((.6*(rJS->GetY())*(rJS->GetY())*(rJS->GetY())) + (.4*(rJS->GetY()))), -((.6*(lJS->GetY())*(lJS->GetY())*(lJS->GetY())) + (.4*(lJS->GetY()))));
			
		//motor for elevator : this works, after the the Great Jaguar Fiasco of 02-18
		//elevatorJaguar->Set(elevatorJS->GetY());  // Assigns elevatorJS to control Jag
				
		
		
		//solenoid control
		
		
	    //Raise and lower arm
		if(driverStation->GetDigitalIn(2) == true) {
			elbowSol->Set(true);     // raise and lower 'elbow'
			//driverStationLCD->Printf(DriverStationLCD::kUser_Line4, 1, "Elbow sol raised"); 
			//driverStationLCD->UpdateLCD();
		}
		else {
			elbowSol->Set(false);
		}
		
		// Open and close gripper
		if(driverStation->GetDigitalIn(3) == true) {
			gripperSol->Set(true);    
			//driverStationLCD->Printf(DriverStationLCD::kUser_Line5, 1, "Gripper sol closed"); 
			//driverStationLCD->UpdateLCD();
		}
		else {
			gripperSol->Set(false);
		}  
		
		// Release pneumatic latch to deploy minibot
		if (driverStation->GetDigitalIn(4) == true){
			deploymentSol->Set(true);
			//driverStationLCD->Printf(DriverStationLCD::kUser_Line6, 1, "Deployment latch triggered"); 
			//driverStationLCD->UpdateLCD();
		}
		else {
			deploymentSol->Set(false);
		}
	
       
  	
		       	
		    	// appears to work [02-19 jaguar problem] copied from 02-17 code	       	
		       if (elevatorJS->GetRawButton(3)== true && outDeploymentLimitSwitch->Get() == true){
		       		deploymentJaguar->Set(1.0);  // Run motor out/forward
		       			 } 
		       			       			
		       else if (elevatorJS->GetRawButton(2)== true && inDeploymentLimitSwitch->Get() == true){
		    	   deploymentJaguar->Set(-1.0); //Run motor in/backwards
		       	}
		       			       			
		       else{
		    	   deploymentJaguar->Set(0.0); // Don't run motor
		       	}

Any thoughts? Thanks!

2

Nothing looks wrong in your code, so it might be some problem with your initialization – or possibly your autonomous function isn’t being called for some reason – use the normal printf and check NetConsole’s output to see if it ever actually happens.

As another suggestion, you can wire limit switches directly to the Jaguar, and it will automatically turn itself off when the switch is pressed. Refer to the Jaguar user’s guide for more information.

3

Thanks for your response.
The rest of our autonomous code is definitely running, so do you think the limit-switch if-statement itself is not getting called? Here is our initialization:

We understand that it’s not legal to control a Jag [actually, now we are using Victors] directly using a limit switch unless you are using CAN [which looked to be a bigger headache than it’s worth] as per J.

When we originally encountered this problem, we planned to workaround it by just running the elevator motor up in autonomous until we reach the scoring grid: we figured that we would reach the scoring grid about the same time the elevator reached the top and started to stall. This would have worked very nicely, except that we recently replaced our wimpy little window-motor with a Fischer-Price motor that cleanly sheared our elevator belt the other day when we ran it just a bit too far! Now I am very wary of running autonomous that way unless I scale the motor back to about 20% rather than 100%. I eagerly await our Week 4 regional…

Thanks again for any help you can give.

#include "WPILib.h"
//#include "Vision/AxisCamera.h"
//#include "Vision/HSLImage.h"
#include "DriverStationLCD.h"
#include <math.h>

// mapping of solenoids to ports
#define GRIPPER_SOL 1
#define ELBOW_SOL 2
#define DEPLOYMENT_SOL 3

class Team956 : public IterativeRobot
{
	
	RobotDrive *robotDrive;		// robot will use PWM 1-4 for drive motors
	
	// Declare a variable for the driver station object
	DriverStation *driverStation;	// driver station object
	DriverStationLCD *driverStationLCD; // Driver station LCD for printouts
	UINT32 priorPacketNumber;	    // keep track of the most recent packet number from the DS
	UINT8 dsPacketsReceivedInCurrentSecond;	// keep track of the ds packets received in the current second
	
	
	Joystick *rJS;         // joystick 1
	Joystick *lJS;         // joystick 2 
	Joystick *elevatorJS;  //controls elevator for gripper 
	
	//PWM *rMotor1;    //interferes with RobotDrive object
	//PWM *rMotor2;
	//PWM *lMotor1;
	//PWM *lMotor2;   
	
	Victor *elevatorVictor; //elevator delivery system for gripper, y-axis, controlled by joystick
	Victor *deploymentVictor; // Motor for deployment of minibot
	
	
	Solenoid *gripperSol; //opens and closes  gripper 
	Solenoid *elbowSol; //raises and lowers 'elbow'
	Solenoid *deploymentSol; // Opens and closes 'latch' for surgical tubing
	
	Relay *compressorRelay; //relay for compressor
		
	DigitalInput *pneumaticLimitSwitch; // shuts off compressor when PSI = 120
	DigitalInput *lLineTracker;			// line tracking sensors
	DigitalInput *mLineTracker;
	DigitalInput *rLineTracker;
	DigitalInput *outDeploymentLimitSwitch; // Shuts off deployment victor if max extension
	DigitalInput *inDeploymentLimitSwitch; // Min extension reached
	DigitalInput *topElevatorLimitSwitch; 
	DigitalInput *bottomElevatorLimitSwitch;
	
	
	
	DigitalInput *rEnCA; //Right encoder channel A
	DigitalInput *rEnCB; //Right encoder channel B
	DigitalInput *lEnCA; //Left  encoder channel A
	DigitalInput *lEnCB; //Left  encoder channel B  
	DigitalInput *elevatorEncoderCA;
	DigitalInput *elevatorEncoderCB;
	
	Encoder *rEncoder; //Right encoder
	Encoder *lEncoder; //Left encoder
	
	
//	AxisCamera &camera; //Camera now wired directly to radio
	
	
	
	// Local variables to count the number of periodic loops performed
	UINT32 autoPeriodicLoops;
	UINT32 disabledPeriodicLoops;
	UINT32 telePeriodicLoops;
	
	enum state {DRIVE=0, STOP=1,SCORE=2, BACKUP=3}; 
	state autoState;
	int leftValue;	
	int middleValue;
	int rightValue;
	 
	int autoFlag;
	int aState;
	
	int LencVal;
	int RencVal;
	
	float speed;
	
	bool atCross; 
		
public:

 // Constructor for the "Team956" Class.

	Team956(void) /* :	camera(AxisCamera::GetInstance())*/
	{
		printf("Team956 Constructor Started
");

		// Create a robot using standard right/left robot drive on PWMS 1, 2, 3, and #4
		robotDrive = new RobotDrive(1,2,3,4); //used to be(1, 3, 2, 4);  changed for Left on PWM's 3,4 and Right on 1,2
		robotDrive->SetSafetyEnabled(false); //disables MotorSafety watchdog
		
		// Acquire the Driver Station object 
		driverStation = DriverStation::GetInstance();
		driverStationLCD = DriverStationLCD::GetInstance();
		priorPacketNumber = 0;
		dsPacketsReceivedInCurrentSecond = 0;
		
	// joysticks
		rJS = new Joystick(1);
		lJS = new Joystick(2);
		elevatorJS = new Joystick(3); //controls elevator for gripper 
		
	
	// PWM's
		//rMotor1 = new PWM(1);  // commented out becuase 'RobotDrive initializes motors
		//rMotor2 = new PWM(2);
		//lMotor1 = new PWM(3);
		//lMotor2 = new PWM(4);
			
		elevatorVictor = new Victor(5);
		elevatorVictor->SetSafetyEnabled(false); //disable the Victor MotorSafety watchdog
		deploymentVictor = new Victor(6);
		deploymentVictor->SetSafetyEnabled(false); 
		
	// solonoids
		gripperSol = new Solenoid(GRIPPER_SOL); 
		
		elbowSol = new Solenoid(ELBOW_SOL);
		deploymentSol= new Solenoid(DEPLOYMENT_SOL);
			
		
	// digital i/o
		pneumaticLimitSwitch = new DigitalInput(1);
		rLineTracker = new DigitalInput(2); 
		mLineTracker = new DigitalInput(3);
		lLineTracker = new DigitalInput(4);
		
	    rEnCA = new DigitalInput(5);
		rEnCB = new DigitalInput(6);
		lEnCA = new DigitalInput(7);
		lEnCB = new DigitalInput(8);  
		
		outDeploymentLimitSwitch = new DigitalInput(9);
		inDeploymentLimitSwitch = new DigitalInput(10);
		topElevatorLimitSwitch = new DigitalInput(11);
		bottomElevatorLimitSwitch = new DigitalInput(12);
	
		
		
		//aChannel, bChannel, reverse direction if encoders are oriented backwards (usually false), type of encoding (1x, 2x, 4x)
		rEncoder = new Encoder(rEnCA->GetChannel(),rEnCB->GetChannel(),false,Encoder::k4X);
		lEncoder = new Encoder(lEnCA->GetChannel(),lEnCB->GetChannel(),false,Encoder::k4X);
		
/*		camera.WriteResolution(AxisCamera::kResolution_320x240);
		camera.WriteCompression(20); // }from 2010ImageDemo
		camera.WriteBrightness(0);     
*/		
		GetWatchdog().SetEnabled(false); // User watchdog should be disabled even without this...
		//GetWatchdog().SetExpiration(10);	//sets watchdog expiration to 1 second
		compressorRelay = new Relay(1, Relay::kForwardOnly);

	

		// Initialize counters to record the number of loops completed in autonomous and teleop modes
		autoPeriodicLoops = 0;
		disabledPeriodicLoops = 0;
		telePeriodicLoops = 0;
		
		// autonomous stuff
    	autoState = DRIVE;
    	
		            
	atCross = false; 
	autoFlag = false;  // Lisa, can we comment these out?

		printf("BuiltinDefaultCode Constructor Completed
");
	}
	

	
	/// Init Routines 
	void RobotInit(void) {
		
		printf("RobotInit() completed.
");
	}
	
	void DisabledInit(void) {
		disabledPeriodicLoops = 0;			// Reset the loop counter for disabled mode
	}

	void AutonomousInit(void) {
		autoPeriodicLoops = 0;				// Reset the loop counter for autonomous mode
		rEncoder->Reset();
		lEncoder->Reset(); 
		rEncoder->Start(); // Start getting values from the encoders
		lEncoder->Start();
		driverStationLCD->Clear();
		
	}

	void TeleopInit(void) {
		telePeriodicLoops = 0;				// Reset the loop counter for teleop mode
		dsPacketsReceivedInCurrentSecond = 0;	// Reset the number of dsPackets in current second
		rEncoder->Reset();
		lEncoder->Reset(); 
		rEncoder->Start(); // Start getting values from the encoders
		lEncoder->Start();
		driverStationLCD->Clear();
		
	}
4

Have you tried running JUST that else statement, (running the motor) in autonomous?

That will help you rule out if you just initialized things improperly.

5

Yes, I am pretty sure we have. [Everything is ‘pretty sure’ at this point because I haven’t had access to the robot for three weeks…]
I know we tried running just the if-statement, with no line-following code or anything like that. There may have been a print-to-driver-station or something like that as well, but nothing that should affect it, I believe.

6

We normally test

if (topLimit.Get())
{
do something
}

or

if (!topLimit.get())
{
do something else
}

The code that works is testing == true while the code that does not work is testing == false.

I can’t see anything wrong with the code; I just have the observation that your test is different between the code that is working and the code that is not working. You might want to rewrite the auto code to use a == true test.