Why isn't this working?

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

[efoote868]

Can you put more debug in there? Debug is your friend.

Also, does case 0 run once or never? From a quick examination of the code it looks like
"last_switch_state = servoswitch.Get();"
right before your while loop would result in

"(servoswitch.Get() != last_switch_state)"
returning true always, especially if no or very little time elapses between the two statements.