Skip to content

Commit

Permalink
Updated all auto to be basic for tomorrow
Browse files Browse the repository at this point in the history
  • Loading branch information
@hdjwis
hdjwis committed Mar 16, 2024
1 parent 62baa94 commit 5668da5
Show file tree
Hide file tree
Showing 4 changed files with 74 additions and 43 deletions.
111 changes: 71 additions & 40 deletions src/main/cpp/Autonomous.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,9 +105,10 @@ void Autonomous::PeriodicDoNothing() {
a_AutoState0 = nextState;
}


//BLUE LEFT RED LEFT
void Autonomous::NoteOne() {
a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(.47), units::meter_t(.79), units::degree_t(60.0)));
a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(.47), units::meter_t(-.79), units::degree_t(60.0)));
a_Gyro->Zero(60.0);
a_AutoState1 = kShoot1stNote1;
state_time = gettime_d();
}
Expand All @@ -121,38 +122,46 @@ void Autonomous::PeriodicNoteOne() {
StopSwerves();
break;
case kShoot1stNote1:
a_NoteHandler->indexToShoot();
nextState = kAutoIdle1;
break;
case kGoTo2ndNote1:
a_NoteHandler->collectNote(-0.4, false);
if(a_SwerveDrive -> odometryGoToPose(8.277, -1.912, 0.0)){
nextState = kShoot2ndNote1;
}
break;
case kShoot2ndNote1:
if(a_SwerveDrive -> odometryGoToPose(3.5, -0.5, 0.0)){
if(gettime_d() > state_time + WAIT_TO_SHOOT) {
state_time = gettime_d();
a_NoteHandler->indexToShoot();
nextState = kGoTo3rdNote1;
nextState = kAutoIdle1;
}
break;
case kGoTo3rdNote1:
a_NoteHandler->collectNote(-0.4, false);
if(a_SwerveDrive -> odometryGoToPose(8.277, -.236, 0.0)){
nextState = kShoot3rdNote1;
case kGoTo2ndNote1:
if(gettime_d() > state_time + WAIT_TO_SHOOT) {
state_time = gettime_d();
a_NoteHandler->collectNote(-0.4, false);
if(a_SwerveDrive -> odometryGoToPose(2.902, -1.455, 0.0)){
nextState = kAutoIdle1;
}
}
break;
case kShoot3rdNote1:
if(a_SwerveDrive -> odometryGoToPose(3.5, -.5, 0.0)){
a_NoteHandler->indexToShoot();
nextState = kAutoIdle1;
}
// case kShoot2ndNote1:
// if(a_SwerveDrive -> odometryGoToPose(3.5, -0.5, 0.0)){
// a_NoteHandler->indexToShoot();
// nextState = kGoTo3rdNote1;
// }
// break;
// case kGoTo3rdNote1:
// a_NoteHandler->collectNote(-0.4, false);
// if(a_SwerveDrive -> odometryGoToPose(8.277, -.236, 0.0)){
// nextState = kShoot3rdNote1;
// }
// break;
// case kShoot3rdNote1:
// if(a_SwerveDrive -> odometryGoToPose(3.5, -.5, 0.0)){
// a_NoteHandler->indexToShoot();
// nextState = kAutoIdle1;
// }
}
a_AutoState1 = nextState;
}


//BLUE CENTER RED CENTER
void Autonomous::NoteTwo() {
a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(1.3), units::meter_t(0.0), units::degree_t(0.0)));
a_Gyro->Zero(0.0);
a_AutoState2 = kGoToNote2;
state_time = gettime_d();
}
Expand All @@ -162,25 +171,36 @@ void Autonomous::PeriodicNoteTwo() {
AutoState2 nextState = a_AutoState2;

switch (a_AutoState2) {
case kAutoIdle2:
case kAutoIdle2:
StopSwerves();
break;
case kShootNote2:
if(gettime_d() > state_time + WAIT_TO_SHOOT) {
state_time = gettime_d();
a_NoteHandler->indexToShoot();
nextState = kGoToNote2;
}
break;
case kGoToNote2:
a_SwerveDrive -> odometryGoToPose(2.9, -1.45, 0.0);
nextState = kShootNote2;
if(gettime_d() > state_time + WAIT_TO_SHOOT) {
state_time = gettime_d();
a_NoteHandler->collectNote(-0.4, false);
if(a_SwerveDrive -> odometryGoToPose(2.9, 0.0, 0.0)){
nextState = kAutoIdle2;
}
}
break;
case kShootNote2:
a_NoteHandler->indexToShoot();
nextState = kAutoIdle2;
break;

}
a_AutoState2 = nextState;
}


//BLUE RIGHT RED RIGHT
void Autonomous::NoteThree()
{
a_AutoState3 = kGoToNote3;
a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(.47), units::meter_t(0.79), units::degree_t(0.0)));
a_Gyro->Zero(300.0);
a_AutoState3 = kGoToNote3;
state_time = gettime_d();
}

Expand All @@ -191,19 +211,30 @@ void Autonomous::PeriodicNoteThree() {
case kAutoIdle3:
StopSwerves();
break;
case kGoToNote3:
a_SwerveDrive -> odometryGoToPose(2.9, 1.45, 0.0);
nextState = kShootNote3;
break;
case kShootNote3:
a_NoteHandler->indexToShoot();
nextState = kAutoIdle3;
if(gettime_d() > state_time + WAIT_TO_SHOOT) {
state_time = gettime_d();
a_NoteHandler->indexToShoot();
nextState = kAutoIdle3;
}
break;
case kGoToNote3:
if(gettime_d() > state_time + WAIT_TO_SHOOT) {
state_time = gettime_d();
a_NoteHandler->collectNote(-0.4, false);
if(a_SwerveDrive -> odometryGoToPose(2.9, 1.45, 0.0)){
nextState = kShootNote3;
}
}
break;

}
a_AutoState3 = nextState;
}

//RED LEFT
void Autonomous::NoteFour(){
a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(.47), units::meter_t(0.79), units::degree_t(0.0)));
a_Gyro->Zero(300.0);
a_AutoState4 = kGoToNote4;
state_time = gettime_d();
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/cpp/Robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -197,7 +197,7 @@ void Robot::AutonomousInit() {
}

a_SwerveDrive.unsetHoldAngle();
a_Gyro.Zero(0.0);

std::string SelectedRoute = m_AutoModeSelector.GetSelected(); //assigns value frm smart dashboard to a string variable

a_Autonomous.StartAuto(SelectedRoute); //starts auto from selected route
Expand Down
2 changes: 1 addition & 1 deletion src/main/include/Prefs.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
#define INVERSE_COLLECTOR_BUTTON 4
#define SHOOTER_BUTTON 6
//#define DEFAULT_SHOOTER_ANGLE 36.0

#define WAIT_TO_SHOOT 3.0
// For the competition bot, this line *MUST* be enabled. For the practice bot, comment out this line.
#define COMP_BOT

Expand Down
2 changes: 1 addition & 1 deletion src/main/include/SwerveDrive.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,7 @@ class SwerveDrive // Class to handle the kinematics of Swerve Drive
frc::ProfiledPIDController<units::meters> xProfiledPid;
frc::ProfiledPIDController<units::meters> yProfiledPid;

frc::TrapezoidProfile<units::radian>::Constraints rotationalConstraints{units::radians_per_second_t(4.0), units::radians_per_second_squared_t(4.0)};
frc::TrapezoidProfile<units::radian>::Constraints rotationalConstraints{units::radians_per_second_t(2.0), units::radians_per_second_squared_t(2.0)};
frc::ProfiledPIDController<units::radian> rotProfiledPid;

};

0 comments on commit 5668da5

Please sign in to comment.