Skip to content

Commit

Permalink
Merge pull request #32 from bearbotics2358/LED-TESt
Browse files Browse the repository at this point in the history 
Improvements from Friday
  • Loading branch information
@aldryd
aldryd authored Mar 16, 2024
2 parents a7f02c6 + 5668da5 commit c5eae3f
Show file tree
Hide file tree
Showing 18 changed files with 335 additions and 238 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
31 changes: 23 additions & 8 deletions src/main/cpp/Climber.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
#include "Climber.h"
#include "Prefs.h"

Climber::Climber(int climberMotorID)://, int topLimitSwitchPort)://, int topPort, int bottomPort):
Climber::Climber(int climberMotorID, int topLimitSwitchPort)://, int topLimitSwitchPort)://, int topPort, int bottomPort):
climberMotor(climberMotorID),
//topLimitSwitch(topLimitSwitchPort),
climberPID(0.0, 0.0, 0.0)
topLimitSwitch(topLimitSwitchPort),
climberPID(0.04, 0.0, 0.0)
//m_climberMotorSignal(climberMotor.GetPosition())
{
//m_climberMotorSignal.SetUpdateFrequency(units::frequency::hertz_t(10.0));
Expand All @@ -15,15 +15,26 @@ void Climber::stopClimber(){
}
void Climber::extendClimnber(){

climberPID.SetSetpoint(0.0);//neeed to change from 0.0
climberPID.SetSetpoint(10.0);//neeed to change from 0.0
double dist = GetClimberPosition();
double speed = climberPID.Calculate(dist, 0.0);
double speed = climberPID.Calculate(dist, 10.0);
speed = std::clamp(speed, -.2, .2);
climberMotor.Set(speed);
climberMotor.Set(-speed);
if(climberPID.AtSetpoint() ){
climberMotor.StopMotor();
}
}
void Climber::runClimberUp(){
if(topLimitSwitch.limitSwitchPressed()){
climberMotor.StopMotor();
}
else{
climberMotor.Set(.05);
}
}
void Climber::runClimberDown(){
climberMotor.Set(-.05);
}

void Climber::retractClimber(){
// if(climberPID.AtSetpoint() || topLimitSwitch.limitSwitchPressed()){
Expand All @@ -39,9 +50,13 @@ void Climber::retractClimber(){

}
double Climber::GetClimberPosition(){
return climberMotor.GetPosition().GetValue().value();
// HOOK moved 6 in and the change in turns was below
double conversion = 6.0/(-69.629395- -31.272461);
return (climberMotor.GetPosition().GetValue().value())*conversion;
}
void Climber::setPosition(){
climberMotor.SetPosition(units::angle::turn_t{0.0});
if(topLimitSwitch.limitSwitchPressed()){
climberMotor.SetPosition(units::angle::turn_t{0.0});
}
}

1 change: 1 addition & 0 deletions src/main/cpp/Collector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@ void Collector::indexToShoot() {
indexerMotor.Set(-0.40);
}
void Collector::indexToAmp() {
std::cout << "Shooter Angle True?" << std::endl;
indexerMotor.Set(-.2);
}
void Collector::stopIndexer() {
Expand Down
77 changes: 65 additions & 12 deletions src/main/cpp/LED.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ void LED::Init()
for(i = 0; i < BUFF_SIZE; i++) {
rx_buff[i] = 0;
}
SetTargetType(LED_STAGE_enum::WHITE);
//SetTargetType(LED_STAGE_enum::WHITE);
}

void LED::Update()
Expand Down Expand Up @@ -83,22 +83,75 @@ void LED::ProcessReport()
{
// parse report
// no action needed, no report expected


}

void LED::SetTargetType(LED_STAGE_enum target_type_param)
{
#ifdef COMP_BOT // Not available on the practice bot
// void LED::SetTargetType(LED_STAGE_enum target_type_param)
// {
// #ifdef COMP_BOT // Not available on the practice bot
// char cmd[10];
// strncpy(cmd, "1,1,1\r\n", 8);
// target_type = target_type_param;
// // lazy way to build a message
// cmd[4] = target_type ? '1' : '0';
// m_serial.Write(cmd, strlen(cmd));
// m_serial.Flush();
// #endif
// }

// LED_STAGE_enum LED::GetTargetType()
// {
// return target_type;
// }

void LED::SetWhite() {
char cmd[10];
strncpy(cmd, "1,1,1\r\n", 8);
target_type = target_type_param;
// lazy way to build a message
cmd[4] = target_type ? '1' : '0';
strncpy(cmd, "0,0\r\n", 8);
m_serial.Write(cmd, strlen(cmd));
m_serial.Flush();
#endif
}

LED_STAGE_enum LED::GetTargetType()
{
return target_type;
void LED::SetMSGIdle() {
char cmd[10];
strncpy(cmd, "1,0\r\n", 8);
m_serial.Write(cmd, strlen(cmd));
m_serial.Flush();

}

void LED::SetNoComms() {
char cmd[10];
strncpy(cmd, "2,0\r\n", 8);
m_serial.Write(cmd, strlen(cmd));
m_serial.Flush();

}

void LED::SetNoteOnBoard() {
char cmd[10];
strncpy(cmd, "3,0\r\n", 8);
m_serial.Write(cmd, strlen(cmd));
m_serial.Flush();


}

void LED::SetAngleToNote(float angle) {
char cmd[10];
sprintf(cmd, "4,1,%1.2f\r\n", angle);
printf("%s\n", cmd);
m_serial.Write(cmd, strlen(cmd));
m_serial.Flush();

}

void LED::SetShooterReady() {
char cmd[10];
strncpy(cmd, "5,0\r\n", 8);
m_serial.Write(cmd, strlen(cmd));
m_serial.Flush();

}

43 changes: 0 additions & 43 deletions src/main/cpp/LED_DIO.cpp
View file

This file was deleted.

Loading

0 comments on commit c5eae3f

Please sign in to comment.