Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

added excel sheet logic to shoot on the move #55

Open
wants to merge 2 commits into
base: dev
Choose a base branch
from
Open

added excel sheet logic to shoot on the move #55

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
aaaadd1
added excel sheet logic
rohitalamgari Apr 2, 2022
68ec834
feeder tuning for current intake
rohitalamgari Apr 4, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 18 additions & 9 deletions Competition/src/main/cpp/subsystems/Drivetrain.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -245,15 +245,24 @@ void Drivetrain::assignOutputs()
units::radians_per_second_t rotRPS = units::radians_per_second_t{rot * SwerveConstants::ROTATION_MAX_SPEED_RPS};

if(state.aButtonPressed){
double magnitude = std::sqrt(std::pow(xSpeed, 2) + std::pow(ySpeed, 2));
xSpeed /= magnitude;
ySpeed /= magnitude;
xSpeedMPS = units::meters_per_second_t{xSpeed};
ySpeedMPS = units::meters_per_second_t{ySpeed};
if(rot != 0){
int sign = std::signbit(rot) == 0 ? 1 : -1;
rotRPS = units::radians_per_second_t{SwerveConstants::ROTATION_SLOW_SPEED_RPS};
}
xSpeedMPS = units::meters_per_second_t{xSpeed * SwerveConstants::DRIVE_SLOW_SPEED_MPS};
ySpeedMPS = units::meters_per_second_t{ySpeed * SwerveConstants::DRIVE_SLOW_SPEED_MPS};
rotRPS = units::radians_per_second_t{rot * SwerveConstants::ROTATION_SLOW_SPEED_RPS};


// double magnitude = hypot(xSpeed, ySpeed);
// if (magnitude > 1)
// {
// xSpeed /= magnitude;
// ySpeed /= magnitude;
// xSpeedMPS = units::meters_per_second_t{xSpeed};
// ySpeedMPS = units::meters_per_second_t{ySpeed};
// }

// if(rot != 0){
// int sign = std::signbit(rot) == 0 ? 1 : -1;
// rotRPS = units::radians_per_second_t{rot * SwerveConstants::ROTATION_SLOW_SPEED_RPS};
// }
}
// double heading = getPose_m().Rotation().Degrees().to<double>();
// if (state.bButtonPressed) {
Expand Down
5 changes: 3 additions & 2 deletions Competition/src/main/cpp/subsystems/Feeder.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,12 +95,13 @@ void Feeder::assessInputs()
state.feederState = FeederState::FEEDER_DISABLE;
}

// Calculate instantaneous current
calcCurrent();

}

void Feeder::analyzeDashboard()
{
// Calculate instantaneous current
calcCurrent();
state.reversed = table->GetBoolean("Reverse Feeder?", false);
state.intakeReverseSpeed = table->GetNumber("Intake Reverse Speed", FeederConstants::DEFAULT_INTAKE_SPEED_REVERSE);
state.feederReverseSpeed = table->GetNumber("Feeder Reverse Speed", FeederConstants::DEFAULT_FEEDER_SPEED_REVERSE);
Expand Down
52 changes: 39 additions & 13 deletions Competition/src/main/cpp/subsystems/TurretTracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@ TurretTracker::TurretTracker() : ValorSubsystem()
void TurretTracker::init() {
initTable("TurretTracker");
table->PutBoolean("Use Turret Shoot", false);
table->PutNumber("Joystick Multiplier", ShooterConstants::jMultiplier);
table->PutNumber("Delta Heading", 0);
}

void TurretTracker::setDrivetrain(Drivetrain *dt){
Expand All @@ -31,35 +33,59 @@ void TurretTracker::assessInputs() {
}

void TurretTracker::analyzeDashboard() {
state.jMultiplier = table->GetNumber("Joystick Multiplier", ShooterConstants::jMultiplier);
}

double TurretTracker::tMinusJ(double robotHeading, double turretPos, double jx, double jy)
{
double turretHeading = robotHeading - 90 + turretPos;
if (turretHeading < -180) turretHeading += 360;
if (turretHeading > 180) turretHeading -= 360;

double tx = (shooter->state.distanceToHub) * cos(turretHeading * MathConstants::toRadians);
double ty = (shooter->state.distanceToHub) * sin(turretHeading * MathConstants::toRadians);

jx *= state.jMultiplier;
jy *= state.jMultiplier;

double rx = tx - jx;
double ry = ty - jy;

double turretHeadingDesired = atan2(ry, rx);
double deltaHeading = turretHeading - (turretHeadingDesired * MathConstants::toDegrees);

if (deltaHeading < -180) deltaHeading += 360;
if (deltaHeading > 180) deltaHeading -= 360;

table->PutNumber("Delta Heading", deltaHeading);
return -deltaHeading;
}

void TurretTracker::assignOutputs() {
// state.cachedVX = drivetrain->getKinematics().ToChassisSpeeds().vx.to<double>();
// state.cachedVY = drivetrain->getKinematics().ToChassisSpeeds().vy.to<double>();
// state.cachedVT = drivetrain->getKinematics().ToChassisSpeeds().omega.to<double>();

double tv = shooter->state.tv;

double robotHeading = drivetrain->getPose_m().Rotation().Degrees().to<double>();
double turretPos = shooter->turretEncoder.GetPosition();
double jx = drivetrain->state.leftStickX;
double jy = -1 * (drivetrain->state.leftStickY);

if (tv == 1) {
state.cachedTx = shooter->state.tx;
// 0.75 = limeligh KP
state.target = (-state.cachedTx * 0.75) + shooter->turretEncoder.GetPosition();

state.cachedHeading = drivetrain->getPose_m().Rotation().Degrees().to<double>();
state.cachedX = drivetrain->getPose_m().X().to<double>();
state.cachedY = drivetrain->getPose_m().Y().to<double>();
state.cachedTurretPos = shooter->turretEncoder.GetPosition();

// state.target = -1 * drivetrain->getPose_m().Rotation().Degrees().to<double>() + state.cachedTurretPos;
// atan2(drivetrain->getKinematics().ToChassisSpeeds().vx.to(<double>()), drivetrain->getPose_m().X());
state.target += tMinusJ(robotHeading, turretPos, jx, jy);

state.cachedHeading = robotHeading;
state.cachedX = drivetrain->getPose_m().X().to<double>();
state.cachedY = drivetrain->getPose_m().Y().to<double>();
state.cachedTurretPos = turretPos;
}
else {
if (table->GetBoolean("Use Turret Shoot", false))
state.target = -1 * drivetrain->getPose_m().Rotation().Degrees().to<double>() + state.cachedTurretPos - state.cachedTx;
state.target = -1 * robotHeading + state.cachedTurretPos - state.cachedTx;
else
state.target = shooter->turretEncoder.GetPosition();
state.target = turretPos;
}

if (state.target < -90) {
Expand Down
4 changes: 3 additions & 1 deletion Competition/src/main/include/Constants.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,8 @@ namespace ShooterConstants{
constexpr static double limelightAngle = 50;
constexpr static double hubHeight = 2.64;
constexpr static double limelightHeight = .6075;

const static double jMultiplier = .4;

constexpr static double flywheelKP1 = 0.088; //1
constexpr static double flywheelKI1 = 0;
Expand Down Expand Up @@ -241,7 +243,7 @@ namespace FeederConstants{
constexpr static double DEFAULT_FEEDER_SPEED_REVERSE = -1.0;

constexpr static int CACHE_SIZE = 20;
constexpr static double JAM_CURRENT = 30;
constexpr static double JAM_CURRENT = 20;
}

namespace MathConstants{
Expand Down
5 changes: 4 additions & 1 deletion Competition/src/main/include/subsystems/TurretTracker.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
#include "Drivetrain.h"
#include "Shooter.h"


#ifndef TURRETTRACKER_H
#define TURRETTRACKER_H

Expand All @@ -29,6 +28,8 @@ class TurretTracker : public ValorSubsystem
void analyzeDashboard();
void assignOutputs();

double tMinusJ(double robotHeading, double turretPos, double jx, double jy);

struct x
{
double target;
Expand All @@ -46,6 +47,8 @@ class TurretTracker : public ValorSubsystem

double cachedTurretPos;

double jMultiplier;

} state;


Expand Down