Skip to content

Commit

Permalink
saturday post break
Browse files Browse the repository at this point in the history
  • Loading branch information
@AntarcticaByToto @janhelgeson
AntarcticaByToto authored and janhelgeson committed Mar 7, 2023
1 parent e001bdb commit af928b1
Show file tree
Hide file tree
Showing 6 changed files with 131 additions and 2 deletions.
29 changes: 29 additions & 0 deletions scripts/auto/autos/barrier2_left_with_cube.csv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
reset_odom,barrierLeft,180
elevarm,cone,front,high
time,1000
intake,outtake_cone
time,1000
intake,disabled
trajectory,barrierLeft,barrierStartLeft,180,reversed
intake,intake_cube
elevarm,cone,front,stow,parallel
trajectory,barrierStartLeft,chell,180,reversed
trajectory,chell,chellFar,180,reversed
elevarm,cube,back,ground
trajectory,chellFar,cave,180,reversed
//back to cube
trajectory,cave,farParallelBarrier,180
split,0.35,multiplier
elevarm,cube,back,stow,parallel
split,0.35,multiplier
trajectory,farParallelBarrier,chellFar,180
trajectory,chellFar,chell,180
trajectory,chell,chellNear,180
//split,0.25,multiplier
//elevarm,cube,front,high,parallel
//split,0.25,multiplier
//trajectory,chell,barrierBox,180
//time,1000
//intake,outtake_cube
//time,1000
//intake,disabled
29 changes: 29 additions & 0 deletions scripts/auto/autos/barrier2_right_with_cube.csv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
reset_odom,barrierRight,180
elevarm,cone,front,high
time,1000
intake,outtake_cone
time,1000
intake,disabled
trajectory,barrierRight,barrierStartRight,180,reversed
intake,intake_cube
elevarm,cone,front,stow,parallel
trajectory,barrierStartRight,chell,180,reversed
trajectory,chell,chellFar,180,reversed
elevarm,cube,back,ground
trajectory,chellFar,cave,180,reversed
//back to cube
trajectory,cave,farParallelBarrier,180
split,0.35,multiplier
elevarm,cube,back,stow,parallel
split,0.35,multiplier
trajectory,farParallelBarrier,chellFar,180
trajectory,chellFar,chell,180
trajectory,chell,chellNear,180
//split,0.25,multiplier
//elevarm,cube,front,high,parallel
//split,0.25,multiplier
//trajectory,chell,barrierBox,180
//time,1000
//intake,outtake_cube
//time,1000
//intake,disabled
29 changes: 29 additions & 0 deletions scripts/auto/autos/wall2_left_with_cube.csv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
reset_odom,wallLeft,180
elevarm,cone,front,high
time,1000
intake,outtake_cone
time,1000
intake,disabled
trajectory,wallLeft,wallStartLeft,180,reversed
intake,intake_cube
elevarm,cone,front,stow,parallel
trajectory,wallStartLeft,glados,180,reversed
trajectory,glados,gladosFar,180,reversed
elevarm,cube,back,ground
trajectory,gladosFar,doug,180,reversed
//back to cube
trajectory,doug,farParallelWall,180
split,0.35,multiplier
elevarm,cube,back,stow,parallel
split,0.35,multiplier
trajectory,farParallelWall,gladosFar,180
trajectory,gladosFar,glados,180
trajectory,glados,gladosNear,180
//split,0.25,multiplier
//elevarm,cube,front,high,parallel
//split,0.25,multiplier
//trajectory,glados,wallBox,180
//time,1000
//intake,outtake_cube
//time,1000
//intake,disabled
29 changes: 29 additions & 0 deletions scripts/auto/autos/wall2_right_with_cube.csv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
reset_odom,wallRight,180
elevarm,cone,front,high
time,1000
intake,outtake_cone
time,1000
intake,disabled
trajectory,wallRight,wallStartRight,180,reversed
intake,intake_cube
elevarm,cone,front,stow,parallel
trajectory,wallStartRight,glados,180,reversed
trajectory,glados,gladosFar,180,reversed
elevarm,cube,back,ground
trajectory,gladosFar,doug,180,reversed
//back to cube
trajectory,doug,farParallelWall,180
split,0.35,multiplier
elevarm,cube,back,stow,parallel
split,0.35,multiplier
trajectory,farParallelWall,gladosFar,180
trajectory,gladosFar,glados,180
trajectory,glados,gladosNear,180
//split,0.25,multiplier
//elevarm,cube,front,high,parallel
//split,0.25,multiplier
//trajectory,glados,wallBox,180
//time,1000
//intake,outtake_cube
//time,1000
//intake,disabled
15 changes: 13 additions & 2 deletions src/main/cpp/subsystems/Elevarm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,9 @@
#define WRIST_FORWARD_LIMIT 325.0f
#define WRIST_REVERSE_LIMIT -325.0f

#define CANCODER_OFFSET 58.8f
#define CANCODER_OFFSET 251.0f

#define INTIAL_HEIGHT_OFFSET 3.0f //cm

#define CARRIAGE_K_F 0.000156f
#define CARRIAGE_K_P 1e-4f
Expand Down Expand Up @@ -197,7 +199,7 @@ void Elevarm::init()
posMap[ElevarmPieceState::ELEVARM_CONE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_GROUND] =frc::Pose2d(-0.99_m, 0.55_m, -208.5_deg);
posMap[ElevarmPieceState::ELEVARM_CONE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_GROUND_TOPPLE] =frc::Pose2d(0.151_m, 0.150_m, 141.4_deg);
posMap[ElevarmPieceState::ELEVARM_CONE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_GROUND_SCORE] =frc::Pose2d(-0.428_m, 0.451_m, -71.0_deg);
posMap[ElevarmPieceState::ELEVARM_CONE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_PLAYER] =frc::Pose2d(-0.8_m, 1.45_m, 57.5_deg);
posMap[ElevarmPieceState::ELEVARM_CONE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_PLAYER] =frc::Pose2d(-0.8_m, 1.485_m, 57.5_deg);
posMap[ElevarmPieceState::ELEVARM_CONE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_MID] =frc::Pose2d(-0.904_m, 1.09_m, -180.0_deg);
posMap[ElevarmPieceState::ELEVARM_CONE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_HIGH] =frc::Pose2d(-0.904_m, 1.09_m, -180.0_deg);
posMap[ElevarmPieceState::ELEVARM_CONE][ElevarmDirectionState::ELEVARM_BACK][ElevarmPositionState::ELEVARM_SNAKE] =frc::Pose2d(-0.904_m, 1.09_m, -180.0_deg);
Expand All @@ -215,6 +217,8 @@ void Elevarm::init()
table->PutNumber("Arm Rotate Max Manual Speed", MAN_MAX_ROTATE);
table->PutBoolean("Pit Mode", futureState.pitModeEnabled);
table->PutNumber("Carraige Stall", carriageStallPower);
table->PutNumber("Carraige Offset", 0);
table->PutBoolean("Enable Carraige Offset", false);

resetState();
armRotateMotor.setEncoderPosition((armCANcoder.GetAbsolutePosition() - CANCODER_OFFSET) / CANCODER_GEAR_RATIO + 180.0);
Expand Down Expand Up @@ -275,6 +279,12 @@ void Elevarm::analyzeDashboard()
manualMaxArmSpeed = table->GetNumber("Arm Rotate Max Manual Speed", MAN_MAX_ROTATE);
futureState.pitModeEnabled = table->GetBoolean("Pit Mode", false);
carriageStallPower = table->GetNumber("Carriage Stall Power", P_MIN_CARRIAGE);

if (table->GetBoolean("Enable Carraige Offset", false)) {
futureState.carraigeOffset = table->GetNumber("Carraige Offset", 0);
} else {
futureState.carraigeOffset = 0;
}
// armStallPower = table->GetNumber("Arm Stall Power", P_MIN_ARM);
futureState.resultKinematics = forwardKinematics(Elevarm::Positions(carriageMotors.getPosition(), armRotateMotor.getPosition(), wristMotor.getPosition()));
futureState.frontMinAngle = minAngle(true);
Expand Down Expand Up @@ -405,6 +415,7 @@ Elevarm::Positions Elevarm::reverseKinematics(frc::Pose2d pose, ElevarmSolutions
height = pose.Y().to<double>() + (X_ARM_LENGTH * std::cos(theta));
}
height -= (Z_CARRIAGE_FLOOR_OFFSET + Z_CARRIAGE_JOINT_OFFSET);
height += futureState.carraigeOffset / 100.0; //convert fron cm on dashboard to m in logic


return Positions(height,theta * 180.0 / M_PI, pose.Rotation().Degrees().to<double>());
Expand Down
2 changes: 2 additions & 0 deletions src/main/include/subsystems/Elevarm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,8 @@ class Elevarm : public ValorSubsystem
double frontMinAngle;
double backMinAngle;

double carraigeOffset;

} futureState, previousState;

double heightDeadband, rotationDeadband;
Expand Down

0 comments on commit af928b1

Please sign in to comment.