Skip to content

Commit

Permalink
Added autos back
Browse files Browse the repository at this point in the history
  • Loading branch information
@VyaasBaskar
VyaasBaskar committed Sep 18, 2024
1 parent 4ec3d25 commit 5ef788f
Show file tree
Hide file tree
Showing 23 changed files with 296 additions and 237 deletions.
36 changes: 18 additions & 18 deletions src/frc846/cpp/frc846/other/trajectory_generator.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,36 +54,36 @@ Trajectory GenerateTrajectory(

for (unsigned int i = input_points.size() - 1; i > 0; --i) {
auto interpolated_points = InterpolatePoints(
input_points[i].pos.point, input_points[i - 1].pos.point,
input_points[i].pos.bearing, input_points[i - 1].pos.bearing, cut);
input_points[i].point, input_points[i - 1].point,
input_points[i].bearing, input_points[i - 1].bearing, cut);
interpolated_points.erase(interpolated_points.begin());

trajectory.push_back({input_points[i].pos});
trajectory.push_back({input_points[i]});

for (auto point : interpolated_points) {
point.velocity[0] = robot_max_v;
trajectory.push_back({point});
}
}
trajectory.push_back({input_points[0].pos});
trajectory.push_back({input_points[0]});

for (unsigned int i = 1; i < trajectory.size(); ++i) {
auto delta_pos =
(trajectory[i].pos.point - trajectory[i - 1].pos.point).magnitude();
(trajectory[i].point - trajectory[i - 1].point).magnitude();

// v₂² = v₁² + 2aΔx
// 2aΔx = v₂² - v₁²
// a = (v₂² - v₁²) / (2Δx)
auto deceleration =
(units::math::pow<2>(trajectory[i].pos.velocity.magnitude()) -
units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude())) /
(units::math::pow<2>(trajectory[i].velocity.magnitude()) -
units::math::pow<2>(trajectory[i - 1].velocity.magnitude())) /
(2 * delta_pos);
if (deceleration > robot_max_deceleration) {
// v₂² = v₁² + 2aΔx
// v₂² = sqrt(v₁² + 2aΔx)

trajectory[i].pos.velocity[0] = units::math::sqrt(
units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude()) +
trajectory[i].velocity[0] = units::math::sqrt(
units::math::pow<2>(trajectory[i - 1].velocity.magnitude()) +
2 * robot_max_deceleration * delta_pos);
}
}
Expand All @@ -92,20 +92,20 @@ Trajectory GenerateTrajectory(

for (unsigned int i = 1; i < trajectory.size(); ++i) {
auto delta_pos =
(trajectory[i].pos.point - trajectory[i - 1].pos.point).magnitude();
(trajectory[i].point - trajectory[i - 1].point).magnitude();

// v₂² = v₁² + 2aΔx
// 2aΔx = v₂² - v₁²
// a = (v₂² - v₁²) / (2Δx)
auto acceleration =
(units::math::pow<2>(trajectory[i].pos.velocity.magnitude()) -
units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude())) /
(units::math::pow<2>(trajectory[i].velocity.magnitude()) -
units::math::pow<2>(trajectory[i - 1].velocity.magnitude())) /
(2 * delta_pos);
if (acceleration > robot_max_acceleration) {
// v₂² = v₁² + 2aΔx
// v₂² = sqrt(v₁² + 2aΔx)
trajectory[i].pos.velocity[0] = units::math::sqrt(
units::math::pow<2>(trajectory[i - 1].pos.velocity.magnitude()) +
trajectory[i].velocity[0] = units::math::sqrt(
units::math::pow<2>(trajectory[i - 1].velocity.magnitude()) +
2 * robot_max_acceleration * delta_pos);

// trajectory[i].v =
Expand All @@ -117,10 +117,10 @@ Trajectory GenerateTrajectory(

// If any point has 0 speed, just set it to the previous speed.
for (unsigned int i = 0; i < trajectory.size(); ++i) {
if (trajectory[i].pos.velocity.magnitude() == 0_fps) {
trajectory[i].pos.velocity.magnitude() =
i == 0 ? trajectory[1].pos.velocity.magnitude()
: trajectory[i - 1].pos.velocity.magnitude();
if (trajectory[i].velocity.magnitude() == 0_fps) {
trajectory[i].velocity.magnitude() =
i == 0 ? trajectory[1].velocity.magnitude()
: trajectory[i - 1].velocity.magnitude();
}
}

Expand Down
21 changes: 21 additions & 0 deletions src/frc846/include/frc846/base/loggable.h
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#pragma once

#include <frc/smartdashboard/SmartDashboard.h>
#include <networktables/NetworkTableInstance.h>
#include <units/base.h>

Expand Down Expand Up @@ -43,6 +44,26 @@ class Loggable {
error_count_++;
}

// Puts a double entry on the smart dashboard.
void Graph(std::string key, double value) {
frc::SmartDashboard::PutNumber(name_ + "/" + key, value);
}

// Puts an integer entry on the smart dashboard.
void Graph(std::string key, int value) {
frc::SmartDashboard::PutNumber(name_ + "/" + key, value);
}

// Puts a boolean entry on the smart dashboard.
void Graph(std::string key, bool value) {
frc::SmartDashboard::PutBoolean(name_ + "/" + key, value);
}

// Puts a string entry on the smart dashboard.
void Graph(std::string key, std::string value) {
frc::SmartDashboard::PutString(name_ + "/" + key, value);
}

static unsigned int GetWarnCount();
static unsigned int GetErrorCount();

Expand Down
4 changes: 1 addition & 3 deletions src/frc846/include/frc846/other/trajectory_generator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,7 @@

namespace frc846 {

struct Waypoint {
frc846::math::FieldPoint pos;
};
using Waypoint = frc846::math::FieldPoint;

using Trajectory = std::vector<Waypoint>;

Expand Down
4 changes: 4 additions & 0 deletions src/frc846/include/frc846/robot/GenericRobot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@ class GenericRobot : public frc::RobotBase, public frc846::base::Loggable {

virtual void OnInitialize() = 0;

virtual void OnDisable() = 0;

virtual void OnPeriodic() = 0;

virtual void InitTeleop() = 0;
virtual void InitTest() = 0;

Expand Down
55 changes: 32 additions & 23 deletions src/y2024/cpp/FunkyRobot.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
#include "commands/basic/deploy_intake_command.h"
#include "commands/basic/shoot_command.h"
#include "commands/basic/spin_up_command.h"
#include "commands/complex/stow_zero_action.h"
#include "commands/follow_trajectory_command.h"
#include "commands/teleop/bracer_command.h"
#include "commands/teleop/drive_command.h"
Expand Down Expand Up @@ -59,47 +60,55 @@ void FunkyRobot::OnInitialize() {
container_.wrist_.Brake();
}));

frc2::Trigger on_coast_trigger{[&] { return coasting_switch_.Get(); }};

on_coast_trigger.OnTrue(frc2::InstantCommand([&] {
container_.pivot_.Coast();
container_.telescope_.Coast();
})
.WithTimeout(1_s)
.AndThen(frc2::WaitCommand(7_s).ToPtr())
.AndThen(frc2::InstantCommand([&] {
container_.pivot_.Brake();
container_.telescope_.Brake();
}).ToPtr()));

frc2::Trigger homing_trigger{[&] { return homing_switch_.Get(); }};

homing_trigger.OnTrue(frc2::InstantCommand([&] {
container_.wrist_.ZeroSubsystem();
container_.pivot_.ZeroSubsystem();
container_.telescope_.ZeroSubsystem();
}).ToPtr());

AddAutos(five_piece_auto_red.get(),
{five_piece_auto_blue.get(), one_piece_auto_0.get(),
one_piece_auto_1.get(), one_piece_auto_2.get(),
one_piece_auto_3.get(), drive_auto_.get()});
}

void FunkyRobot::OnDisable() {
container_.pivot_.Brake();
container_.wrist_.Brake();
container_.telescope_.Brake();
}

void FunkyRobot::InitTeleop() {
container_.pivot_.Brake();
container_.telescope_.Brake();
container_.wrist_.Brake();
container_.wrist_.Coast();

container_.drivetrain_.SetDefaultCommand(DriveCommand{container_});
container_.control_input_.SetDefaultCommand(
OperatorControlCommand{container_});
container_.super_structure_.SetDefaultCommand(StowCommand{container_});
container_.super_structure_.SetDefaultCommand(
StowZeroActionCommand{container_});
container_.intake_.SetDefaultCommand(IdleIntakeCommand{container_});
container_.shooter_.SetDefaultCommand(IdleShooterCommand{container_});
container_.bracer_.SetDefaultCommand(BracerCommand{container_});

ControlTriggerInitializer::InitTeleopTriggers(container_);
}

void FunkyRobot::OnPeriodic() {
Graph("homing_switch", homing_switch_.Get());
Graph("coasting_switch", coasting_switch_.Get());

if (homing_switch_.Get()) {
container_.super_structure_.ZeroSubsystem();
}

if (coast_counter_ >= 1) {
coast_counter_--;
} else if (coast_counter_ == 0) {
container_.pivot_.Brake();
container_.wrist_.Brake();
container_.telescope_.Brake();
} else if (coasting_switch_.Get()) {
container_.pivot_.Coast();
container_.wrist_.Coast();
container_.telescope_.Coast();
coast_counter_ = start_coast_counter_.value();
}
}

void FunkyRobot::InitTest() {}
18 changes: 7 additions & 11 deletions src/y2024/cpp/autos/drive_auto.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,12 @@ DriveAuto::DriveAuto(RobotContainer& container)
container, "drive_auto",
frc2::SequentialCommandGroup{

// frc2::InstantCommand{[&] {
// auto pose_ = field::points::kTestingOrigin(false);
// container.drivetrain_.SetPoint(pose_.point);
// container.drivetrain_.SetBearing(pose_.bearing);
// }},
// FollowTrajectoryCommand{
// container,
// {
// {field::points::kTestingPoint(false), 0_fps},
// },
// }
frc2::InstantCommand{[&] {
auto pose_ = field::points.kTestingOrigin();
container.drivetrain_.SetPoint(pose_.point);
container.drivetrain_.SetBearing(pose_.bearing);
}},
FollowTrajectoryCommand{container,
{field::points.kTestingPoint()}}

}} {}
75 changes: 39 additions & 36 deletions src/y2024/cpp/autos/five_piece_auto.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,52 +4,55 @@
#include <frc2/command/ParallelDeadlineGroup.h>
#include <frc2/command/ParallelRaceGroup.h>
#include <frc2/command/SequentialCommandGroup.h>
#include <frc2/command/WaitCommand.h>
#include <frc2/command/WaitUntilCommand.h>

#include "commands/auto_intake_and_shoot_command.h"
#include "commands/basic/auto_shoot_command.h"
#include "commands/basic/prepare_auto_shoot_command.h"
#include "commands/basic/spin_up_command.h"
#include "commands/basic/wrist_zero_command.h"
#include "commands/follow_trajectory_command.h"
#include "commands/teleop/stow_command.h"
#include "field.h"
#include "frc2/command/WaitCommand.h"

FivePieceAuto::FivePieceAuto(RobotContainer& container, bool should_flip)
: frc846::robot::GenericCommandGroup<RobotContainer, FivePieceAuto,
frc2::SequentialCommandGroup>{
container, should_flip ? "5p_auto_blue" : "5p_auto_red",
frc2::SequentialCommandGroup{

// frc2::InstantCommand{[&, flip = should_flip] {
// auto pose_ = field::points::kFivePieceOrigin(flip);
// container.drivetrain_.SetPoint(pose_.point);
// container.drivetrain_.SetBearing(flip ? 180_deg : 0_deg);
// }},
// PrepareAutoShootCommand{container},
// AutoShootCommand{container},
// frc2::WaitCommand{
// container.super_structure_.post_shoot_wait_.value()},

// AutoIntakeAndShootCommand(
// container,
// {field::points::kFivePieceIntakeOne(should_flip), 0_fps},
// {field::points::kFivePieceMidOne(should_flip), 15_fps},
// {field::points::kFivePieceShootOne(should_flip), 0_fps}),

// AutoIntakeAndShootCommand(
// container,
// {field::points::kFivePieceIntakeTwo(should_flip), 0_fps},
// {field::points::kFivePieceMidTwo(should_flip), 15_fps},
// {field::points::kFivePieceShootTwo(should_flip), 0_fps}),

// AutoIntakeAndShootCommand(
// container,
// {field::points::kFivePieceIntakeThree(should_flip),
// 0_fps},
// {field::points::kFivePieceMidThree(should_flip), 15_fps},
// {field::points::kFivePieceShootThree(should_flip),
// 0_fps}),

// FollowTrajectoryCommand(
// container,
// {{field::points::kFivePieceIntakeOne(should_flip),
// 0_fps}})
}} {}
frc2::InstantCommand{[&, flip = should_flip] {
auto pose_ = field::points.kFivePieceOrigin(flip);
container.drivetrain_.SetPoint(pose_.point);
container.drivetrain_.SetBearing(flip ? 180_deg : 0_deg);
}},
frc2::ParallelDeadlineGroup{
frc2::ParallelDeadlineGroup{
frc2::WaitUntilCommand{[&] {
return container_.pivot_.WithinTolerance(
container_.super_structure_.getStowSetpoint()
.pivot); /* homing wrist at start of auto */
}},
StowCommand{container}},
SpinUpCommand{
container}}, /* start to spin up shooter to save time */
WristZeroCommand{container}, /* homing the wrist */
PrepareAutoShootCommand{container}, AutoShootCommand{container},
frc2::WaitCommand{
container.super_structure_.post_shoot_wait_.value()},

AutoIntakeAndShootCommand(
container, field::points.intake_one_path(should_flip),
{field::points.kFivePieceOrigin(should_flip)}),

AutoIntakeAndShootCommand(
container, field::points.intake_two_path(should_flip),
{field::points.kFivePieceOrigin(should_flip)}),

AutoIntakeAndShootCommand(
container, field::points.intake_three_path(should_flip),
{field::points.kFivePieceOrigin(should_flip)}),

FollowTrajectoryCommand(
container, {field::points.kFivePieceFinish(should_flip)})}} {}
8 changes: 4 additions & 4 deletions src/y2024/cpp/commands/auto_intake_and_shoot_command.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,19 +13,19 @@
#include "frc2/command/WaitCommand.h"

AutoIntakeAndShootCommand::AutoIntakeAndShootCommand(
RobotContainer& container, frc846::Waypoint intake_point,
frc846::Waypoint mid_point, frc846::Waypoint shoot_point)
RobotContainer& container, std::vector<frc846::Waypoint> intake_path,
std::vector<frc846::Waypoint> shoot_path)
: frc846::robot::GenericCommandGroup<RobotContainer,
AutoIntakeAndShootCommand,
frc2::SequentialCommandGroup>{
container, "auto_intake_shoot_command",
frc2::SequentialCommandGroup{

AutoDeployIntakeCommand{container},
FollowTrajectoryCommand{container, {mid_point, intake_point}},
FollowTrajectoryCommand{container, intake_path},
frc2::ParallelDeadlineGroup{
frc2::SequentialCommandGroup{
FollowTrajectoryCommand{container, {shoot_point}},
FollowTrajectoryCommand{container, shoot_path},
frc2::WaitCommand(
container.super_structure_.pre_shoot_wait_.value())},
PrepareAutoShootCommand{container}},
Expand Down
2 changes: 1 addition & 1 deletion src/y2024/cpp/commands/basic/shoot_command.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,5 +24,5 @@ bool ShootCommand::IsFinished() {
num_loops_ = 0;
}

return num_loops_ > 20;
return num_loops_ > 70;
}
Loading

0 comments on commit 5ef788f

Please sign in to comment.