Need Assistance with making autonmous work.

[thatprogrammer]

Hey, in our auto, we need two limit switches to be pressed. After they get pressed, the robot would need to go back. The distance code works very well, but I can't seem to get it to correspond to a limit press. Also, the autonomous height was reported as 0 before, but I MIGHT have fixed that. (I had noticed a missing variable, never tested the elevator after that)

#include "WPILib.h"

class Robot: public IterativeRobot
{
private:
	//Initializing livewindow
	LiveWindow *lw;
	//Initializing stick
	Joystick *stick, *Joy, *OtherJoy;
	//Initializing the Chassis parts
	Talon *kFrontLeftMotor;
	Talon *kFrontRightMotor;
	Talon *Elevator;
	RobotDrive *robot;
	Relay *Fan;
	DoubleSolenoid *shifter;
	Encoder *ChassisEnc;
	Encoder *OtherEnc;
	AnalogInput *ElevatorEnc;
	DigitalInput *LightLimit, *OtherLightLimit;
	Solenoid *Light, *OtherLight;
	//Initializing the values for Cheesy Drive
	float Rotation;
	float Acceleration;
	float rightMultiplier;
	float leftMultiplier;
    double automode;
    Gyro *gyro;
    bool robottime;
    double Angle;
    DoubleSolenoid *ElevSol;
    bool robotdrive;
   double Kp = 0.08;
   DigitalInput *Limit, *TopLimit, *OtherLimit;
   PIDController *ElevatorPID;
   double ElevatorKp;
   double ElevatorKd;
   double ElevatorHeight;
   double HeightError;
   double HeightSum;
   double Goal;
   double Out;
   double threshold;
   double numRotations;
   double lastAngle;
   double absAngle;
   double ActualRotation;
   bool Stop;
   bool TopStop;
  double DistancePerNum;
   double LastElevatorHeight;
   double Power;
   bool ShiftOpened;
   bool Goto;
   double SuperRotation;
   double Ultra;
   bool Lighton;
   bool OtherLighton;
   bool ElevSaftey;
	void RobotInit()
	{

		SmartDashboard::init();
		lw = LiveWindow::GetInstance();
		stick = new Joystick(0);
		Joy = new Joystick (2);
		OtherJoy = new Joystick(1);
		kFrontLeftMotor = new Talon(0);
		kFrontRightMotor = new Talon(1);
		Elevator = new Talon (2);
		robot = new RobotDrive(kFrontRightMotor, kFrontLeftMotor);
		Fan = new Relay (3);


		/* Setting the shifter as a DoubleSolenoid
		 * Because we're using both pistons off of
		 * one Double Solenoid
		 */
		shifter = new DoubleSolenoid (0,1);

		ChassisEnc = new Encoder (0,1, false, Encoder::EncodingType::k4X);
		OtherEnc= new Encoder (8,9, false, Encoder::EncodingType::k4X);
        ElevatorEnc = new AnalogInput (3);
		//Setting it so the fan is off by default
		Fan->Set(Relay::kOff);


		//Setting the Rotation and Accel values
		Rotation = stick->GetRawAxis(1);
		Acceleration = stick->GetRawAxis(3);

	    /*Setting the multipliers
	     * so that they don't allow
	     * a robot to go full forward
	     * while going full turn
	     */

		rightMultiplier = Rotation + Acceleration;
		leftMultiplier = Rotation - Acceleration;

		//Setting the shifter to Low Gear
		shifter->Set(DoubleSolenoid::kReverse);
		robot->SetInvertedMotor(RobotDrive::kFrontLeftMotor, true);
		robot->SetInvertedMotor(RobotDrive::kFrontRightMotor, true);

		ChassisEnc->SetDistancePerPulse(.084);
		OtherEnc->SetDistancePerPulse(.084);
	gyro = new Gyro (1);
	Angle = gyro->GetAngle();

	if (stick->GetRawButton(1))

		{
			automode = 1;
		}
		if (stick->GetRawButton(2))
		{
			automode = 2;
		} else {
			automode = 0;
		}
	SmartDashboard::PutNumber("Auto:", automode);
gyro->InitGyro();
ElevSol = new DoubleSolenoid (2,3);
gyro->InitGyro();
Limit = new DigitalInput(7);
TopLimit = new DigitalInput(3);
OtherLimit = new DigitalInput(2);
DistancePerNum = .05;
Goto = false;
Goal = 0;
Light = new Solenoid (4);
OtherLight = new Solenoid(5);
LightLimit = new DigitalInput(4);
OtherLightLimit = new DigitalInput (5);

//Out = ElevatorKp *HeightError + ElevatorKd *HeightSum;
//Out = (.5);
//ElevatorPID = new PIDController(0.5, 0.0, 0.0, ElevatorEnc, Elevator);
//ElevatorPID->Enable();


	}



	void AutonomousInit()
	{

		gyro->Reset();
kFrontRightMotor->SetSafetyEnabled(false);
kFrontLeftMotor->SetSafetyEnabled(false);
ChassisEnc->Reset();
threshold = 340;
absAngle = ElevatorEnc->GetVoltage() * 72;
if ( absAngle- lastAngle < -threshold) {
          numRotations++;
        	   } else if (absAngle - lastAngle > threshold){
        		   numRotations--;
        	   }
if (numRotations<0)
{
	numRotations = 0;
}
ActualRotation = ((340 * numRotations) + absAngle)- 300;
		        LastElevatorHeight = ElevatorHeight;
		        HeightError = Goal - ElevatorHeight;
		        HeightSum = ElevatorHeight - LastElevatorHeight;
		        ElevatorKp = .75;
		        ElevatorKd = .0;
		        SuperRotation = (360 * numRotations) - absAngle;
		        Power = ElevatorKp*HeightError + ElevatorKd*HeightSum;
		        ElevatorHeight= (ActualRotation/37.24)  ;
		        		if (ElevatorHeight < 0){
		        			ElevatorHeight =0;
		        		}
		        		ElevSol->Set(DoubleSolenoid::kForward);

	}

	void AutonomousPeriodic()
	{
		bool OtherDrive;
		if ( absAngle- lastAngle < -threshold) {
		          numRotations++;
		        	   } else if (absAngle - lastAngle > threshold){
		        		   numRotations--;
		        	   }
		absAngle = ElevatorEnc->GetVoltage() * 72;
		ElevatorHeight= (ActualRotation/37.24) ;
				if (ElevatorHeight < 0){
					ElevatorHeight =0;
				}
				Ultra = ((ActualRotation + absAngle - 600) + 4);
				ActualRotation = ((threshold * numRotations) + absAngle)- 300;
				        LastElevatorHeight = ElevatorHeight;
				        HeightError = Goal - ElevatorHeight;
				        HeightSum = ElevatorHeight - LastElevatorHeight;
				        ElevatorKp = .75;
				        ElevatorKd = .0;
				        SuperRotation = (360 * numRotations) - absAngle;
				        Power = ElevatorKp*HeightError + ElevatorKd*HeightSum;


		SmartDashboard::PutNumber("Angle", gyro->GetAngle());
		SmartDashboard::PutNumber("AutoHeight", ElevatorHeight);
		double Distace = ChassisEnc->GetDistance();
		float Angle = gyro->GetAngle();
        bool turner;
        bool Grab;
        bool Auton1;
        int Driveto;
		bool ElevSaftey;
		int DriveBack =0;
        Auton1 = true;
        if (OtherLimit->Get()||TopLimit->Get())
        {
        	ElevSaftey = true;
        } else {
        	ElevSaftey = false;
        }
        //bool Auton2;
        //bool Auton3;
        if (Goto==true)
                	{
                		Elevator->Set(Power);

                		if (Power > .68){
                			Power = .68;
                		}
                		if (Power < -.68)
                		{
                			Power = -.68;
                		}
                	}
        if (Auton1 == true){

        	 if (OtherLimit->Get() || TopLimit->Get())
        	        {
        	        	ElevSaftey = true;
        	        } else {
        	        	ElevSaftey = false;
        	        }
        	 if (ElevSaftey == true)




        	 {
        		 Elevator->Set(0);
        		 Goto = false;
        	 }
  Grab = true;
  if (Grab == true){


	  ElevSol->Set(DoubleSolenoid::kForward);
 Driveto = 0;

  }
  if (LightLimit->Get() || OtherLightLimit->Get()){
//	  Wait(2);
	  DriveBack= Driveto + 1;
	  Goto = true; }
	  if (Goto == true){
		  Grab = false;
       if (Grab == false)
       {
    	   if (DriveBack>0){
    		   DriveBack = 1;
    	   }
       }
	  }


  if (Grab == false && Goto == true)
  {


	 /* if (Goto==true){
		  Goal =5; }
	          else if (Goto== true && ElevSaftey == true)
	          {
	        	  Elevator->Set(0);
	          }*/


 	 if (OtherLimit->Get() || TopLimit->Get())
 	        {
 	        	ElevSaftey = true;
 	        } else {
 	        	ElevSaftey = false;
 	        }
 	 if (ElevSaftey == true)
 	 {
 		 Elevator->Set(0);

 		        		 Goto = false;
 		        	 }




	  if (ElevatorHeight>= Goal){
	    	Driveto = true;
	Goto= false;
	    }
	  if (ElevSaftey== true && Driveto == true)
	  {
		  Elevator->Set(0);
		  Goto = false;
	  } else if (ElevSaftey == true && Driveto == false)
	  {
		  Goto = true;
     	 if (OtherLimit->Get() || TopLimit->Get())
     	        {
     	        	ElevSaftey = true;
     	        } else {
     	        	ElevSaftey = false;
     	        }
     	 if (ElevSaftey == true)
     	 {
     		 Elevator->Set(0);


     	 		        		 Goto = false;

     	 }
	  }


  }

  if (DriveBack > 0)
		  {
	  if (Distace > -108 ){
	  	  		  robot->Drive(.5, -Angle*Kp);

	  	  		  Goto = false;
	  }

	  if (Distace <=108 && DriveBack >0 )
	  {
		  robot->Drive(0, 0);
		  DriveBack = 0;
		  OtherDrive = true;
		 /* if (ElevSaftey == true)
		  {
			  Elevator->Set(0);
			  OtherDrive = true;
		  } else {
			  Elevator->Set(-.5);
		  } */
  if (OtherDrive == true){
	  if (Distace < 112)
	  {
		  robot->Drive(.5, 0);
	  } else if (Distace > 112)
	  {
		  robot->Drive(0, 0);
	  }
  }

  }
	  SmartDashboard::PutNumber("AutoHeight", ElevatorHeight);

		  }}}