Possessed robot

[harryji369]

Can anyone help us, Our programmer wrote a function for our Elevator Mechanism to get a certain heigt by using he inches per second that the arm go at the push of a button. Our problem is that while the Elevator Mechanism is getting that height we would lose control of the robot untill the arm is finished getting to that height, so if we were going a full speed while it is getting a height the robot would keep going at full speed even if we let go of the drive joysticks.

[Straberrie]

What type of a function did your programmer use for the arm? Is it inside a separate while loop? Last year, if I used a while loop for anything inside the operator control, I would have to insert the Tank Drive code inside it because everything outside that while loop would get ignored. So, if you have teleop mode and a while inside, everything inside the while loop will operatre but nothing outside of it will work until the code exited the loop.

[krudeboy51]

Originally Posted by Straberrie

What type of a function did your programmer use for the arm? Is it inside a separate while loop? Last year, if I used a while loop for anything inside the operator control, I would have to insert the Tank Drive code inside it because everything outside that while loop would get ignored. So, if you have teleop mode and a while inside, everything inside the while loop will operatre but nothing outside of it will work until the code exited the loop.

so to what I understand from this is that when the function is called it will exit the operator while loop and complete the function before going back into the loop?

[Alan Anderson]

Your programmer has probably written the "extend elevator" code as a loop waiting for the elevator to get to the desired location. A loop like that "stalls" the rest of the code. I'm surprised your robot isn't shutting down due to a communication watchdog timeout.

What you'll need to have instead is to start the motor and do a one-time computation of a global variable defining how long to run the motor when the button is first pressed, then a quick test in the teleop code to see if the time has elapsed and stop the motor if it has. Teleop is already called repeatedly from inside a loop.

[davidthefat]

I know exactly the problem: make a boolean that triggers false when the arm action is initiated and set the PWMs to 127. So when the action is done, the boolean is turned true and you can drive again. The boolean has to be set to true again in the Arm function when the whole arm thing is done.

bool Drive = true;
{
if(Drive == false)
{
PWMSetNeutral();
Arm();
}
else
{
Drive();
}
Drive = Joystick->getRawButton(1);
}

[krudeboy51]

Originally Posted by Alan Anderson

Your programmer has probably written the "extend elevator" code as a loop waiting for the elevator to get to the desired location. A loop like that "stalls" the rest of the code. I'm surprised your robot isn't shutting down due to a communication watchdog timeout.

What you'll need to have instead is to start the motor and do a one-time computation of a global variable defining how long to run the motor when the button is first pressed, then a quick test in the teleop code to see if the time has elapsed and stop the motor if it has. Teleop is already called repeatedly from inside a loop.

I disabled the watchdog, and the time I have is determined by the distance divided by the inches per second and the inches per second is a const static double which is declared in public. The arm gets the height right on the dime, perfectly but the only thing is that our driver cannot control the robot while its doing that.

[davidthefat]

Are you using the "Drive" function given to you by WPI? I suggest you look at the source code and see how it operates. You will then see why it is happening. That is why I rarely use functions that other people make.

[krudeboy51]

Originally Posted by davidthefat

Are you using the "Drive" function given to you by WPI? I suggest you look at the source code and see how it operates. You will then see why it is happening. That is why I rarely use functions that other people make.

I costumly made the functions, one function I made was a drive function which makes the robot drive straight by taking 3% of speed off of the right motor, so i would call this function when I want to drive straight

[Alan Anderson]

Kory, if you'll post the section of code you're using to make the elevator go up when the button is pressed, I'll show you exactly how to "unwrap" the loop to let it work simultaneously with your drive motor control. Make sure to include the part at the very beginning where it reads the button.

[davidthefat]

Originally Posted by krudeboy51

I costumly made the functions, one function I made was a drive function which makes the robot drive straight by taking 3% of speed off of the right motor, so i would call this function when I want to drive straight

Yes, do the thing I said in my previous post. I had the exact same problem. You set the PWMs to 127 before you deploy

[krudeboy51]

class RobotDemo : public SimpleRobot

Victor *Elevator_motor;
Victor *Carriage_motor;
const static double velocity=49.25; //8ft per sec, 
			/*steps to calculate velocity
			 * 1. run test code using thirdstick trigger
			 * 2.measrure the robots distance in feet
			 * time = 4 seconds
			 * 3. velocity= feet/time
			 *
			 */
const static double time_To_Turn_360=4;
			/*example
			 * 180degrees * 1fullcircle/360degrees= fraction of full circle
			 * =180/360=1/2*time turn turn
			 */
double last_carriageheight;
double last_elevatorheight;
const static double Carriage_velocity=11.25;
const static double Elevator_velocity=8.5;

void OperatorControl(void)
{
	myRobot.SetSafetyEnabled(false);
		
	while (IsOperatorControl())
	{
	     Drive(Leftstick.GetY(), Rightstick.GetY()); 			     Wait(0.005);

if (ds->GetDigitalIn(1) != 1)	//MIDDLE'S TOP SCORING POSITION
			{
				arm_setposition(1);
			}
			
			if (ds->GetDigitalIn(2) != 1)   //LEFT OR RIGHT'S TOP SCORING POSITION
			{
				arm_setposition(2);
			
			}
			
			if (ds->GetDigitalIn(3) != 1)	//MIDDLE'S MID SCORING POSITION
			{
				arm_setposition(3);
			}
			
			if (ds->GetDigitalIn(4) != 1)	//LEFT OR RIGHT'S MIDDLE SCORING POSITION
			{
				arm_setposition(4);
			}
			
			if (ds->GetDigitalIn(5) != 1)	//MIDDLE'S BOTTOM SCORING POSITION 
			{
				arm_setposition(5);
			}
			
			if (ds->GetDigitalIn(6) != 1)	//LEFT OR RIGHT'S BOTTOM SCORING POSITION
			{
				arm_setposition(6);
			}
			
			if (ds->GetDigitalIn(7) != 1)	//FEEDER POSITION
			{
				arm_setposition(7);
			}
			
       }
};

void arm_setposition(int pos)
	{
		
		switch(pos)
		{
		case 1:
			Drive(Leftstick.GetY(), Rightstick.GetY());
			Carriage_getheight(37);
			Elevator_getheight(42);
			break;
			
		case 2:
			Drive(Leftstick.GetY(), Rightstick.GetY());
			Carriage_getheight(35);
			Elevator_getheight(40);
			break;
			
		case 3:
			Drive(Leftstick.GetY(), Rightstick.GetY());
			Carriage_getheight(32);	
                                      Elevator_getheight(9);
			break;
			
		case 4:
			Drive(Leftstick.GetY(), Rightstick.GetY());
			Carriage_getheight(32);
			Elevator_getheight(4);
			break;
			
		case 5:
			Drive(Leftstick.GetY(), Rightstick.GetY());
			Carriage_getheight(0);
			Elevator_getheight(0);
			break;
			
		case 6:
			Drive(Leftstick.GetY(), Rightstick.GetY());
			Carriage_getheight(0);
			Elevator_getheight(0);
			break;
			
		case 7:
			
			Drive(Leftstick.GetY(), Rightstick.GetY());
			Carriage_getheight(32);
			Elevator_getheight(13);
			break;
			
		}
	}


void Robot_driveDistance(double dist)
	{
		double speed = -0.75;
		double time=dist/velocity;
		if (dist < 0)
		{
			speed = 0.5;
			time = time * -1;
		}
		Drive(speed,speed);
		Wait(time);
		Drive(0.0,0.0);
	}

	void Carriage_getheight(double height)
	{
		double motor_speed = 1.0;
		double time=height/Carriage_velocity;
		if(height < 0)
		{
			motor_speed = motor_speed * -1;
			time = time * -1;
		}
		Carriage_motor->Set(motor_speed);
		Wait(time);
		Carriage_motor->Set(0.0);
	}
	
	void Elevator_getheight(double height)
	{
		double motor_speed = 1.0;
		double time=height/Elevator_velocity;
		if(height < 0)
		{
			motor_speed = motor_speed * -1;
			time = time * -1;
		}
		Elevator_motor->Set(motor_speed);
		Wait(time);
		Elevator_motor->Set(0.0);
	}
	
	void Drive(double right_motor_speed, double left_motor_speed)
	{
right_motor_speed = right_motor_speed - right_motor_speed *3/100;                                
		myRobot.TankDrive(right_motor_speed, left_motor_speed);

	}

another problem that we figured out was that whenever the camera is connected to the robot and we enable it, it would stay enabled for 3 seconds then disconnect and we would get an error on windriver.
I took a pic. of the problem on windriver

[davidthefat]

Eh, not exactly "custom" function... Personally, I feel that there is no point in using C++ if you are not going to use classes per se. I think it defeats the whole purpose of using an object oriented language. Yes, I know the library is essentially a whole bunch of classes and straight C would not recognize it. Personally, I would say clean up the code. I am not saying it is not "dirty" (it can be optimized...) but it is congested. I learned the very hard way, once when a fairly good programmer gave me a lecture on not splitting up the source into separate files and classes and other last year, when I essentially wrote spaghetti code, the inspector and mentor was appalled by it. Robotics does tend to make you write any code that "works" over organized, optimized and clean code that works.

Ok, see what I wrote before (it's not going to run as is, it is essentially pseudo code) and pay attention to why I do certain things. I will not spoon feed you.

[Greg McKaskle]

The reason the robot is possessed is that you are calling Wait inside if Elevator_get_the_heigh and Carriage_get_the_height. The tasking thread is told to wait for some time and cannot return back to get the next teleop packet containing new joystick values. The packets where the drivers let go of the joystick are being sent to the robot, but they are ignored because of the Wait().

There are many ways to fix this, but it is very easy to introduce. First make sure the explanation makes sense to you and any other programmers working on the robot.

Assuming time can be driven by the ~20ms arrival rate of new teleop packets, what you want to do is record the time that you started the carriage or elevator motor in a static variables, or perhaps if you do want to use objects, create a static object that holds the pending movements of the motors, the time movement started, and any other info that is helpful.

Each time you enter into teleop, do a quick check to see if a motor has been on long enough and should be turned off. This is also when you'd turn on the elevator motor.

Pay special attention to the initial values and any overrides that the drivers may need that turn the motors off before they reach their target.

Once you get rid of the waits, the exorcism should be complete, and perhaps you can even turn the watchdog back on. It will help identify any other placed in your code which wait or loop and block communications.

Greg McKaskle