Skip to content

Commit

Permalink
Merge pull request #41 from bearbotics2358/two-note-auto
Browse files Browse the repository at this point in the history 
Two note auto
  • Loading branch information
@hdjwis
hdjwis authored Mar 26, 2024
2 parents 5900a52 + 12546e2 commit 4af5ca3
Show file tree
Hide file tree
Showing 12 changed files with 374 additions and 208 deletions.
23 changes: 17 additions & 6 deletions src/main/cpp/AmpTrap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@ rollerMotor(rollerMotorID),
extensionMotor(extensionMotorID),
rotationMotor(rotationMotorID),
//.08 .008
extendPID(0.3, 0.05, 0.0),
extendPID(0.3, 0.03, 0.0),
rotationPID(0.0, 0.0, 0.0),
a_BeamBreak(AMP_BEAM_BREAK_PORT) ,
// m_rotationMotorSignal(rotationMotor.GetPosition()),
Expand All @@ -27,7 +27,7 @@ a_ArmAngle(1)

}
void AmpTrap::runRoller(){
rollerMotor.Set(-0.25);
rollerMotor.Set(-0.32);
}
void AmpTrap::stopRoller(){
rollerMotor.StopMotor();
Expand All @@ -37,14 +37,22 @@ bool AmpTrap::extendExtender(double goal){
double dist = GetExtensionPosition();
double speed = extendPID.Calculate(dist, goal);
speed = std::clamp(speed, -1.0, 1.0);
extensionMotor.Set(-speed);
if(fabs(dist-goal) < .05){

frc::SmartDashboard::PutNumber("extension goal", 1000000000000);
if(fabs(dist-goal) < .25){
frc::SmartDashboard::PutString("a;owieguh;", "oiahg;r");
extensionMotor.StopMotor();
return true;
}
return false;
else{
extensionMotor.Set(-speed);
return false;
}

}
void AmpTrap::stopExtension(){
extensionMotor.StopMotor();
}

double AmpTrap::GetExtensionPosition(){
double conversion = (-3.0)/(.30975 + 19.312988);
Expand All @@ -70,6 +78,9 @@ bool AmpTrap::moveToPosition(double desiredaAngle){
return false;
}
}
void AmpTrap::stopArm(){
rotationMotor.StopMotor();
}
double AmpTrap::GetArmAngle(){
a_ArmAngle.Update();
return a_ArmAngle.GetAngle();
Expand All @@ -86,4 +97,4 @@ void AmpTrap::setPID(double p, double i, double d){
rotationPID.SetP(p);
rotationPID.SetI(i);
rotationPID.SetD(d);
}
}
158 changes: 96 additions & 62 deletions src/main/cpp/Autonomous.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,9 +107,9 @@ void Autonomous::PeriodicDoNothing() {

//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_Gyro->Zero(60.0);
a_AutoState1 = kShoot1stNote1;
a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(0.0), units::meter_t(0.0), units::degree_t(0.0)));
a_Gyro->Zero(0.0);
a_AutoState1 = kRotateToShoot1;
state_time = misc::gettime_d();
}

Expand All @@ -118,82 +118,99 @@ void Autonomous::PeriodicNoteOne() {
AutoState1 nextState = a_AutoState1;

switch (a_AutoState1) {
case kAutoIdle1:
case kAutoIdle1:
StopSwerves();
break;
case kShoot1stNote1:
if(misc::gettime_d() > state_time + WAIT_TO_SHOOT) {
case kRotateToShoot1:
if(a_SwerveDrive->odometryGoToPose(0.5, 0.0, 5*(M_PI/3))){
state_time = misc::gettime_d();
a_NoteHandler->indexToShoot();
nextState = kGoTo2ndNote1;
nextState = kShootFirstNote1;
}
break;
case kGoTo2ndNote1:
if(misc::gettime_d() > state_time + WAIT_TO_SHOOT) {
case kShootFirstNote1:
if(misc::gettime_d() > state_time + 1.0){
a_NoteHandler->shootNote(-.25);
state_time = misc::gettime_d();
a_NoteHandler->collectNote(-0.4, false);
a_SwerveDrive->swerveUpdate(.1, .1, 0.0, true);
nextState = kGoToSecondNote1;
}
else{
nextState = kAutoIdle1;
break;
case kGoToSecondNote1:
a_NoteHandler->collectNote(-.4, true);
if(misc::gettime_d() > state_time + 1.0){
if(a_SwerveDrive->odometryGoToPose(-1.602, 0.665, 0.0)){
state_time = misc::gettime_d();
nextState = kGoToSpeaker1;
}
}
break;
// 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;
// }
}
case kGoToSpeaker1:
if(a_SwerveDrive->odometryGoToPose(0.83, 0.0, 5*(M_PI/3))){
state_time = misc::gettime_d();
nextState = kShootSecondNote1;
}
break;
case kShootSecondNote1:
a_NoteHandler->shootNote(-.25);
if(misc::gettime_d() > state_time + 1.0){
state_time = misc::gettime_d();
nextState = kAutoIdle1;
}
break;
}
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_Gyro->Zero(300.0);
a_AutoState2 = kShootNote2;
a_Gyro->Zero(0.0);
//a_Gyro->Zero(300.0);
a_AutoState2 = kShootFirstNote2;
state_time = misc::gettime_d();
}

void Autonomous::PeriodicNoteTwo() {

AutoState2 nextState = a_AutoState2;



switch (a_AutoState2) {
case kAutoIdle2:
StopSwerves();
break;
case kShootNote2:
if(misc::gettime_d() > state_time + WAIT_TO_SHOOT){
a_NoteHandler->indexToShoot();
if(misc::gettime_d() > state_time + 2*(WAIT_TO_SHOOT)) {
case kShootFirstNote2:

if(misc::gettime_d() > state_time + 1.0){
a_NoteHandler->shootNote(-.25);
state_time = misc::gettime_d();
nextState = kGoToSecondNote2;
}
break;
case kGoToSecondNote2:
a_NoteHandler->collectNote(-.4, true);
if(misc::gettime_d() > state_time + 1.0){
if(a_SwerveDrive->odometryGoToPose(-.3, 0.0, 0.0)){
state_time = misc::gettime_d();
nextState = kGoToNote2;
nextState = kGoToSpeaker2;
}
}
break;
case kGoToNote2:
if(misc::gettime_d() < state_time + 2*(WAIT_TO_SHOOT)) {
a_NoteHandler->collectNote(-0.4, false);
a_SwerveDrive->swerveUpdate(0.0, -0.15, 0.0, true);
case kGoToSpeaker2:
if(a_SwerveDrive->odometryGoToPose(1.3, 0.0, 0.0)){
state_time = misc::gettime_d();
nextState = kShootSecondNote2;
}
else{
break;
case kShootSecondNote2:
a_NoteHandler->shootNote(-.25);
if(misc::gettime_d() > state_time + 1.0){
state_time = misc::gettime_d();
nextState = kAutoIdle2;
}
break;



}
a_AutoState2 = nextState;
Expand All @@ -202,44 +219,61 @@ void Autonomous::PeriodicNoteTwo() {
//BLUE RIGHT RED RIGHT
void Autonomous::NoteThree()
{
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;
a_SwerveDrive->zeroPose(frc::Pose2d(units::meter_t(0.0), units::meter_t(0.0), units::degree_t(0.0)));
a_Gyro->Zero(0.0);
a_AutoState3 = kRotateToShoot3;
state_time = misc::gettime_d();
}

void Autonomous::PeriodicNoteThree() {
AutoState3 nextState = a_AutoState3;
frc::SmartDashboard::PutNumber("Auto State", nextState);

switch (a_AutoState3) {
case kAutoIdle3:
case kAutoIdle3:
StopSwerves();
break;
case kShootNote3:
if(misc::gettime_d() > state_time + WAIT_TO_SHOOT){
a_NoteHandler->indexToShoot();
if(misc::gettime_d() > state_time + 2*(WAIT_TO_SHOOT)) {
case kRotateToShoot3:
if(a_SwerveDrive->odometryGoToPose(0.5, 0.0, M_PI/3)){
state_time = misc::gettime_d();
nextState = kShootFirstNote3;
}
break;
case kShootFirstNote3:
if(misc::gettime_d() > state_time + 1.0){
a_NoteHandler->shootNote(-.25);
state_time = misc::gettime_d();
nextState = kGoToSecondNote3;
}
break;
case kGoToSecondNote3:
a_NoteHandler->collectNote(-.4, true);
if(misc::gettime_d() > state_time + 1.0){
if(a_SwerveDrive->odometryGoToPose(-1.602, -0.665, 0.0)){
state_time = misc::gettime_d();
nextState = kGoToNote3;
nextState = kGoToSpeaker3;
}
}
break;
case kGoToNote3:
if(misc::gettime_d() < state_time + 2*(WAIT_TO_SHOOT)) {
a_NoteHandler->collectNote(-0.4, false);
a_SwerveDrive->swerveUpdate(-0.1, -0.15, 0.0, true);
case kGoToSpeaker3:
if(a_SwerveDrive->odometryGoToPose(0.5, 0.0, M_PI/3)){
state_time = misc::gettime_d();
nextState = kShootSecondNote3;
}
else{
break;
case kShootSecondNote3:
a_NoteHandler->shootNote(-.25);
if(misc::gettime_d() > state_time + 1.0){
state_time = misc::gettime_d();
nextState = kAutoIdle3;
}
break;

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_SwerveDrive->zeroPose();
a_Gyro->Zero(300.0);
a_AutoState4 = kGoToNote4;
state_time = misc::gettime_d();
Expand Down
42 changes: 31 additions & 11 deletions src/main/cpp/Climber.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,26 +4,46 @@
Climber::Climber(int climberMotorID, int topLimitSwitchPort)://, int topLimitSwitchPort)://, int topPort, int bottomPort):
climberMotor(climberMotorID),
topLimitSwitch(topLimitSwitchPort),
climberPID(0.04, 0.0, 0.0)
climberPID(0.05, 0.006, 0.0)
//m_climberMotorSignal(climberMotor.GetPosition())
{
climberPID.SetTolerance(1.0);
//m_climberMotorSignal.SetUpdateFrequency(units::frequency::hertz_t(10.0));
climberMotor.SetNeutralMode(1);
}
void Climber::stopClimber(){
climberMotor.StopMotor();
}
void Climber::extendClimnber(){

climberPID.SetSetpoint(10.0);//neeed to change from 0.0
double dist = GetClimberPosition();
double speed = climberPID.Calculate(dist, 10.0);
speed = std::clamp(speed, -.2, .2);
climberMotor.Set(-speed);
if(climberPID.AtSetpoint() ){
climberMotor.StopMotor();
}
bool Climber::extendClimnber(double position){
climberPID.SetSetpoint(position);//neeed to change from 0.0
double dist = GetClimberPosition();
double speed = climberPID.Calculate(dist, position);
speed = std::clamp(speed, -1.0, 1.0);
/* Code that I believe is equivalent.
if(topLimitSwitch.limitSwitchPressed()) {
stopClimber();
} else if(-speed < 0.0) {
climberMotor.Set(-speed);
}
*/
if(-speed > 0.0){
if(topLimitSwitch.limitSwitchPressed()){
stopClimber();
}
else{
climberMotor.Set(-speed);
}
}
else{
climberMotor.Set(-speed);
}
if(fabs(dist - position) < .25){
climberMotor.StopMotor();
return true;
}
return false;
}

void Climber::runClimberUp(){
if(topLimitSwitch.limitSwitchPressed()){
climberMotor.StopMotor();
Expand Down
Loading

0 comments on commit 4af5ca3

Please sign in to comment.