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);

}}}

[Christopher149]

Just to be sure, do you know if the switches work? They should have a connection between signal and ground on a DIO port.

[thatprogrammer]

Just to be sure, do you know if the switches work? They should have a connection between signal and ground on a DIO port.

They work 100% in teleop. Also, is there a way to make the robot wait until the piston is done opening before it starts with the rest of auto?

[GeeTwo]

.. is there a way to make the robot wait until the piston is done opening before it starts with the rest of auto?

You'll need some sort of sensor to detect when that happens. If your pistons have ferromagnetic plates, you can place magnetic switches on the outside of the pistons to detect when the piston is "done". These switches are usually provided by Bimba with the FIRST choice cylinders, or can be purchased easily enough. If your pistions are non-magnetic, a limit switch is your best bet. These can be triggered mechanically or through magnetism, or even optically. Figuring out what style of limit switch you need is very dependent on your application. If you can't think of how to place a limit switch on your device, I suggest going to google images and searching for "limit switch". Look at the pictures until something looks like it could work, or 100 pictures, whichever comes first.

FWIW, the 3946 robot has three distinctive styles of limit switch this year; two optical, six push-button, and four lever arm with roller.

[rich2202]

Two options:

1) Put a magnetic sensor on the piston. Then you can tell when it has moved to the desired location.

2) Put a time delay, and wait 1 second, or however long it takes the piston to move.