Chief Delphi - Why isn't this working?

Code:

#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.