Will this autonomous code work?

[Mandelbrot]

Our build team has been working on the robot for quite a while and we haven't had time to check to see if our code will work. I don't think it's likely that we will have time soon, so I wanted to see if any of you on Chief Delphi would be able to spot any problems with it right off the bat.

This code is supposed to make the robot drive 100 inches and then stop, slowing down after every sixth of the target distance it has traveled.

void OperatorControl(void)
	{
		float 	X, Y, T, angle;
		float	armPosition;
		int 	sensors;                        
		double motorX = 0, motorY = 0;
		double targetDistance = 100;            //The total distance that the robot will drive
		double drivePeriod = 0.05;              //Time between motor update
		double printPeriod = 0.25;				//How long in between prints
		double driveExpiration, printExpiration; 
		double powerProfile[] = {0.3, 0.25, 0.2, 0.15, 0.1, 0.09}; //To reduce the robot speed gradually
		double distanceTraveled = 0;
		int speed = 0;                          //The element of the powerProfile matrix
		double currentSpeed = 0;                //The current speed the robot is traveling at
		int goneSixth = 1;                      //How many sixths of the total distance traveled
		double distancePerPulse = .075398223686;  //How many inches the robot travels after one pulse is sensed    
		int pulsePerRevolution = 250;           //The pulses on one revolution of the wheel
		int secPerMin = 60;
//		dashboard.init();
//		dashboard.log("Initializing...", "System State")
		printf("Initializing...\n");
		myRobot.SetSafetyEnabled(true);
		
		gyro.Reset();	// remove this for actual matches where gyro is reset before autonomous
		
		timer.Reset();
		timer.Start();
		
		driveExpiration = drivePeriod;
		printExpiration = printPeriod;
		motorX = 0;
		motorY = -0.1;
		
		while (IsOperatorControl())
		{

else if (stick1.GetRawButton(6) == true)
			{
				timer.Reset();
				timer.Start();
				distanceTraveled = 0;
				
				while (stick1.GetRawButton(6) == true && distanceTraveled <= targetDistance && timer.Get() < 4.0 )
				{
					if (timer.Get() > driveExpiration)
					{
						driveExpiration += drivePeriod;					//Adds drivePeriod to the driveExpiration
						
						//Calculates the current speed
						currentSpeed = ((motorFrontLeft.GetSpeed() + motorFrontRight.GetSpeed() + 
								motorRearLeft.GetSpeed() + motorRearRight.GetSpeed())/4);

						//Calculates the distance traveled
						distanceTraveled += distancePerPulse * (pulsePerRevolution/secPerMin) * currentSpeed * drivePeriod;					
						
						//Decreases the speed after every sixth of the target distance the robot has driven.
						if(distanceTraveled > (targetDistance * (goneSixth/7)))
						{
							speed++;
							goneSixth++;
						}
						myRobot.MecanumDrive_Cartesian(0, -powerProfile[speed], 0, angle);
						
						//printf("Period: %e Distance Traveled: %e   Current Speed:  %e\n" , drivePeriod, distanceTraveled, currentSpeed);
					}
					//Wait(waitPeriod);
					
				}
				myRobot.MecanumDrive_Cartesian(0, 0, 0, angle);             //Stops the robot
				//printf("Finished\n");                           	//Displays when the while loop ends
				while( stick1.GetRawButton(6) == true)
				{
					
				}
			}

Thanks in advance!

[Dacilndak]

Well, first and foremost, all of this needs to be in void Autonomous(void), not void OperatorControl(void). If you upload this code and enable Autonomous, nothing will happen; the code above would run if you enabled Teleoperated and held down button #6 on your joystick.

Other than that, your logic seems sound.

[JLenhart]

Keep in mind that the floor may be slightly elevated in some locations do to the general geography so it may take a little tweaking once you actually get there.

I know that we had to use two different autonomous modes for each side of the field in Palmetto because of this. One side was slightly more elevated than the other.