[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