Off Season Programming Question: Switch Statements

[tomy]

I am trying to implement a switch statement in auto mode. The code goes to the first statement then never goes to the second statement. Any suggestions?

#include "WPILib.h"

class RobotDemo : public SimpleRobot
{
	RobotDrive myRobot;
	Talon motor;
	Joystick stick;
	Encoder en;
	PIDController pid;

	int step;


public:
	RobotDemo(void):
		myRobot(7,8),
		motor (2),
		stick(1),
		en(1,2, true,Encoder::k4X),
		pid(.1, 0, .003, &en, &motor)


	{
		myRobot.SetExpiration(0.1);

		en.Start();
		step = 1;


	}


	void Autonomous(void)
	{
		myRobot.SetSafetyEnabled(false);



		switch (step){
			case 1:
				pid.Enable();
				pid.SetTolerance(.1);
				pid.SetOutputRange(-.5, .5);
				pid.SetSetpoint(497);

				if(pid.OnTarget()){
					step++;
				}

				break;

			case 2:
				pid.Enable();
				pid.SetTolerance(.1);
				pid.SetOutputRange(-.25, .25);
				pid.SetSetpoint(800);

				if(pid.OnTarget()){
					step++;
				}

				break;


			default:
				break;
		}

	}


	void OperatorControl(void)
	{


		myRobot.SetSafetyEnabled(false);

		while (IsOperatorControl() && IsEnabled())
		{



			Wait(0.005);
		}
	}


	void Test() {




	}
};

START_ROBOT_CLASS(RobotDemo);

[Mark McLeod]

Autonomous only gets called once, so the switch statement only gets executed once. You need to add a loop inside Autonomous not unlike the loop you have in OperatorControl.

[tomy]

Once I add the while loop will the method I am using for switching between case statements work?

void Autonomous(void)
	{
		myRobot.SetSafetyEnabled(false);

         
         while(IsAutonomous && IsEnabled){
		switch (step){
			case 1:
				pid.Enable();
				pid.SetTolerance(.1);
				pid.SetOutputRange(-.5, .5);
				pid.SetSetpoint(497);

				if(pid.OnTarget()){
					step++;
				}

				break;

			case 2:
				pid.Enable();
				pid.SetTolerance(.1);
				pid.SetOutputRange(-.25, .25);
				pid.SetSetpoint(800);

				if(pid.OnTarget()){
					step++;
				}

				break;


			default:
				break;
		}
            }
	}

[GeeTwo]

Once you have it in a loop, you'd probably do much better not to be constantly resetting the PID parameters while it's enabled. I'd try something more like:

                    switch (step){
			case 1:
				pid.SetTolerance(.1);
				pid.SetOutputRange(-.5, .5);
				pid.SetSetpoint(497);
				pid.Enable();
                                step++;
				break;

			case 2:
				if(pid.OnTarget()){
					step++;
				}
				break;

			case 3:
                                pid.Disable();
				pid.SetTolerance(.1);
				pid.SetOutputRange(-.25, .25);
				pid.SetSetpoint(800);
				pid.Enable();
                                step++;
				break;

			case 4:
				if(pid.OnTarget()){
					step++;
				}
				break;

			default:
				break;
		}

Or, for brevity at the cost of readability, you could remove the case 4 block and move the case 4 statement up next to the case 2 statement, or remove case 4 entirely and never disable the PID in autonomous if you want to leave it on until the end; in this case, you should put the disable outside the loop.

[tomy]

Originally Posted by GeeTwo

Once you have it in a loop, you'd probably do much better not to be constantly resetting the PID parameters while it's enabled. I'd try something more like:

                    switch (step){
			case 1:
				pid.SetTolerance(.1);
				pid.SetOutputRange(-.5, .5);
				pid.SetSetpoint(497);
				pid.Enable();
                                step++;
				break;

			case 2:
				if(pid.OnTarget()){
					step++;
				}
				break;

			case 3:
                                pid.Disable();
				pid.SetTolerance(.1);
				pid.SetOutputRange(-.25, .25);
				pid.SetSetpoint(800);
				pid.Enable();
                                step++;
				break;

			case 4:
				if(pid.OnTarget()){
					step++;
				}
				break;

			default:
				break;
		}

Or, for brevity at the cost of readability, you could remove the case 4 block and move the case 4 statement up next to the case 2 statement, or remove case 4 entirely and never disable the PID in autonomous if you want to leave it on until the end; in this case, you should put the disable outside the loop.

When I ran that code it went into the first case statement got to the target then stopped. It doesn't seem to want to go into the second case statement.

[GeeTwo]

Originally Posted by tomy

When I ran that code it went into the first case statement got to the target then stopped. It doesn't seem to want to go into the second case statement.

Did you put this in the IsAutonomous && IsEnabled loop?

[tomy]

Originally Posted by GeeTwo

Did you put this in the IsAutonomous && IsEnabled loop?

Yes. I figured out the problem and I'm trying to think of a way to fix it.

case 2:
     if(pid.OnTarget()){
          step++;
     }
     break;

That section of code is not change the value of step. I put "step" on the smart dash board and once it gets to case 2 it doesn't move on to case 3. I also checked the value of the encoder with the smart dashboard and it is getting to get set point.


EDIT: I just put the pid.OnTarget(); on the smart dashboard; it never changes to true. I cant figure out why.

[GeeTwo]

Originally Posted by tomy

I just put the pid.OnTarget(); on the smart dashboard; it never changes to true. I cant figure out why.

Have you checked what value is being returned from the sensor? Perhaps it's close enough to the actual value that the applied drive speed is within the deadband of the controller, but the sensor value is not within 0.5.

[tomy]

Originally Posted by GeeTwo

Have you checked what value is being returned from the sensor? Perhaps it's close enough to the actual value that the applied drive speed is within the deadband of the controller, but the sensor value is not within 0.5.

The sensor value being returned is between 495-497. What do you mean the sensor value is not within 0.5? I thought the tolerance of the setpoint was set by pid.SetTolerance(); and that pid.SetOutPutRange(); was the maximum and minimum speed that you wanted the motor to move.

[GeeTwo]

Originally Posted by tomy

The sensor value being returned is between 495-497. What do you mean the sensor value is not within 0.5? I thought the tolerance of the setpoint was set by pid.SetTolerance(); and that pid.SetOutPutRange(); was the maximum and minimum speed that you wanted the motor to move.

IIRC, SetTolerance defines the allowable "plus-or-minus" for onTarget to return true. If your sensor is returning a value smaller than (or perhaps <=) 496.9, onTarget will continue to report false. Expand your tolerance to 2.0 or a bit higher, and this should let you move on.

Edit, I see that Tolerance is a percentage, so your answer must be above 496.5031 to have onTarget return true.

Re-edit: the percentage is a fraction of the range defined in SetInputRange(), which you have never called, so it defaults to zero. I suggest using SetAbsoluteTolerance() with a value of 2.0 or greater.

[tomy]

That fixed it! Thanks for the help one last question. Shouldn't the input to .SetAbsoluteTolerance() be something like .4 instead of 4? I thought in the code it converted it to a percentage.

[GeeTwo]

SetAbsoluteTolerance() bypasses the percentage calculation, so that you specify the tolerance directly in terms of sensor output. SetTolerance() and SetPercentTolerance() are functionally equivalent and set the tolerance to a percentage of the range specified in the latest call to SetOutputRange(). Since you never called SetOutputRange, my reading of the source code makes me believe that these default to zero, so SetTolerance() will always produce a zero tolerance if SetOutputRange() has not been called.