| thatprogrammer |
15-02-2015 20:01 |
Need Assistance with making autonmous work.
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);
}}}
|
