3rd Robot bring up
Signed-off-by: Filip Kujawa <filip.j.kujawa@gmail.com>
Change-Id: I1bb85cf323276c2239f2ae3af2f33df323162d99
diff --git a/y2023_bot3/control_loops/python/pivot_joint.py b/y2023_bot3/control_loops/python/pivot_joint.py
index c2d94ba..baea920 100644
--- a/y2023_bot3/control_loops/python/pivot_joint.py
+++ b/y2023_bot3/control_loops/python/pivot_joint.py
@@ -13,24 +13,25 @@
FLAGS = gflags.FLAGS
try:
- gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+ gflags.DEFINE_bool("plot", False, "If true, plot the loop response.")
except gflags.DuplicateFlagError:
pass
kPivotJoint = angular_system.AngularSystemParams(
- name='PivotJoint',
- motor=control_loop.BAG(),
- G=(6.0 / 48.0) * (20.0 / 100.0) * (18.0 / 24.0) * (24.0 / 44.0),
+ name="PivotJoint",
+ motor=control_loop.Falcon(),
+ G=(14 / 50) * (24 / 64) * (24 / 64) * (15 / 60),
# Use parallel axis theorem to get the moment of inertia around
# the joint (I = I_cm + mh^2 = 0.001877 + 0.8332 * 0.0407162^2)
- J=0.003258,
+ J=(0.13372 * 2.00),
q_pos=0.80,
q_vel=80.0,
kalman_q_pos=0.12,
kalman_q_vel=2.0,
- kalman_q_voltage=0.5,
+ kalman_q_voltage=1.5,
kalman_r_position=0.05,
- radius=5.71 * 0.0254)
+ radius=5.71 * 0.0254,
+)
def main(argv):
@@ -43,17 +44,17 @@
# Write the generated constants out to a file.
if len(argv) != 5:
glog.fatal(
- 'Expected .h file name and .cc file name for the pivot joint and integral pivot joint.'
+ "Expected .h file name and .cc file name for the pivot joint and integral pivot joint."
)
else:
namespaces = [
- 'y2023_bot3', 'control_loops', 'superstructure', 'pivot_joint'
+ "y2023_bot3", "control_loops", "superstructure", "pivot_joint"
]
angular_system.WriteAngularSystem(kPivotJoint, argv[1:3], argv[3:5],
namespaces)
-if __name__ == '__main__':
+if __name__ == "__main__":
argv = FLAGS(sys.argv)
glog.init()
sys.exit(main(argv))
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.cc b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
index 7669396..bc656fb 100644
--- a/y2023_bot3/control_loops/superstructure/pivot_joint.cc
+++ b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
@@ -26,27 +26,27 @@
break;
case PivotGoal::PICKUP_FRONT:
- pivot_goal = 0.25;
+ pivot_goal = 1.74609993820075;
break;
case PivotGoal::PICKUP_BACK:
- pivot_goal = 0.30;
+ pivot_goal = -1.7763851077235;
break;
case PivotGoal::SCORE_LOW_FRONT:
- pivot_goal = 0.35;
+ pivot_goal = 1.74609993820075;
break;
case PivotGoal::SCORE_LOW_BACK:
- pivot_goal = 0.40;
+ pivot_goal = -1.7763851077235;
break;
case PivotGoal::SCORE_MID_FRONT:
- pivot_goal = 0.45;
+ pivot_goal = 0.846887672907125;
break;
case PivotGoal::SCORE_MID_BACK:
- pivot_goal = 0.5;
+ pivot_goal = -0.763222056740831;
break;
}
@@ -55,7 +55,7 @@
pivot_joint_offset = frc971::control_loops::
CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*status_fbb, pivot_goal,
- frc971::CreateProfileParameters(*status_fbb, 12.0, 90.0));
+ frc971::CreateProfileParameters(*status_fbb, 5.0, 20.0));
status_fbb->Finish(pivot_joint_offset);
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.cc b/y2023_bot3/control_loops/superstructure/superstructure.cc
index 3b4835c..6061a76 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure.cc
@@ -54,7 +54,7 @@
pivot_joint_offset = pivot_joint_.RunIteration(
unsafe_goal != nullptr ? unsafe_goal->pivot_goal()
: PivotGoal::NEUTRAL,
- &(output_struct.pivot_joint_voltage),
+ output != nullptr ? &(output_struct.pivot_joint_voltage) : nullptr,
position->pivot_joint_position(), status->fbb());
Status::Builder status_builder = status->MakeBuilder<Status>();
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
index 1150e06..7b2a3d2 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -177,33 +177,32 @@
break;
case PivotGoal::PICKUP_FRONT:
- pivot_goal = 0.25;
+ pivot_goal = 1.74609993820075;
break;
case PivotGoal::PICKUP_BACK:
- pivot_goal = 0.30;
+ pivot_goal = -1.7763851077235;
break;
case PivotGoal::SCORE_LOW_FRONT:
- pivot_goal = 0.35;
+ pivot_goal = 1.74609993820075;
break;
case PivotGoal::SCORE_LOW_BACK:
- pivot_goal = 0.40;
+ pivot_goal = -1.7763851077235;
break;
case PivotGoal::SCORE_MID_FRONT:
- pivot_goal = 0.45;
+ pivot_goal = 0.846887672907125;
break;
case PivotGoal::SCORE_MID_BACK:
- pivot_goal = 0.5;
+ pivot_goal = -0.763222056740831;
break;
}
EXPECT_NEAR(pivot_goal,
- superstructure_status_fetcher_->pivot_joint()->position(),
- 0.001);
+ superstructure_status_fetcher_->pivot_joint()->position(), 3);
}
}