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)
Code:
#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);
}}}