[duffany1]

#include "WPILib.h"
#include "Utilities.h"
#include "math.h"
// #include "Vision/AxisCamera.h"

const int servo_pwm = 4;
const int switch_one_port = 12;

int state;
UINT32 last_switch_state;
float desired_angle;
float last_recorded_angle;

class RobotDemo : public SimpleRobot
{
	
	Servo mainservo;
	DigitalInput servoswitch;
	Timer servoTimeout;
	
	public:
		RobotDemo(void):
			
		        mainservo(4),
			servoswitch(switch_one_port),
			servoTimeout()
		
			{
				GetWatchdog().SetExpiration(0.1);
				// AxisCamera & camera = AxisCamera::GetInstance();
			}

		void OperatorControl(void)
		{
			GetWatchdog().SetEnabled(true);
			
			state = 0;
			last_switch_state = servoswitch.Get();
			
			while (IsOperatorControl())
			{
				GetWatchdog().Feed();

				switch (state)
				{
				
					case 0: // wait for servo action
						
						if (servoswitch.Get() != last_switch_state)
						{
							state++;
						}
						
						cout << "Waiting for switch action..." << endl;
						
					break;
					
					case 1: // start moving the servo
						
						cout << "Servo has started moving." << endl;
						
						if (servoswitch.Get() == 0)
						{
							mainservo.Set(0.0);
							desired_angle = 0.0;
						}
						else
						{
							mainservo.Set(1.0);
							desired_angle = 1.0;
						}
						
						
						
						servoTimeout.Reset();
						servoTimeout.Start();
						recordAngle();
						state++;
						
					break;
					
					case 2: // servo is moving to desired angle
						
						if (servoTimeout.HasPeriodPassed(0.5))
						{
							
							if (mainservo.Get() != desired_angle)
							{
								if (fabs (mainservo.Get() - last_recorded_angle) < 0.1)
									state++; // the servo is obstructed by an object
							}
							else
							{
								state++; // Servo has moved to the desired angle.
							}
							
							servoTimeout.Reset();
							servoTimeout.Start();
							recordAngle();
							
							cout << "Current angle: " << last_recorded_angle << endl;
							
						}
						
					break;
					
					case 3:
						
						cout << "State is 3." << endl;
						
						mainservo.Set(last_recorded_angle);
					        state = 0;
					    
					break;
					
				}
				
				// cout << mainservo.GetAngle() << endl;
				 
				last_switch_state = servoswitch.Get();
				
			}
		}
		
		void recordAngle()
		{
			last_recorded_angle = mainservo.Get();
		}
};

START_ROBOT_CLASS(RobotDemo);

What I am trying to accomplish is the following:

- I have a servo (which works properly, and is wired properly).
- I want to flick a switch which is declared as a DigitalInput, constructed as DigitalInput 12.
- When I flick this switch I want the servo to move to either full left or full right (depending on its current position. I want it to move opposite of its current position).
- I want to account for objects obstructing the path of this servo. If the servo hasn't reached the position I told it to go to, and it hasn't moved more than 18 degrees in 500 milliseconds, then we can assume there is an object obstructing it, so I want to stop the servo from moving.

I have multiple problems with this program. One problem is that in case 0, state++ never gets executed, and I can't figure out why. Assume that cout is working properly (because I have the cRIO properly connected to another laptop via serial cable). However, I am not sure whether cout messes with anything.

Also, the code just straight up doesn't work. Before, I had managed to jump into case 1 (I kinda had to force it) and even so, my code wasn't working. I would be most grateful if someone could give me a few pointers on what I'm doing wrong, or even if I am attacking this from the right angle.