Got ball control working

misprit7 · Jan 25, 2024 · 0bb5199 · 0bb5199
1 parent b55bc7f
commit 0bb5199
Show file tree

Hide file tree

Showing 4 changed files with 306 additions and 119 deletions.
diff --git a/software/algo.cpp b/software/algo.cpp
@@ -39,9 +39,6 @@ state_t algo_action(params){
     return algo_fns[cur_state](cur_state, ball_pos, ball_vel, mtr_cmds);
 }
 
-/*
- * Gets index of player currently closest to target_cm
- */
 int closest_plr(int rod, double target_cm, double cur_pos){
     int plr1 = clamp((int)floor(num_plrs[rod] * target_cm / play_height), 0, num_plrs[rod]-1);
     double plr1_pos = bumper_width + plr_width/2 + plr1*plr_gap[rod] + cur_pos;
@@ -59,11 +56,10 @@ int closest_plr(int rod, double target_cm, double cur_pos){
         return plr1;
 }
 
-/*
- * Gets rod closest to ball_cm
- * ret[0]: side_t
- * ret[1]: rod_t
- */
+double plr_offset(int plr, int rod){
+    return bumper_width + plr_width/2 + plr*plr_gap[rod];
+}
+
 pair<side_t, rod_t> closest_rod(double ball_cm){
     if(ball_cm >= 3*rod_gap) return {human, goalie};
     if(ball_cm >= 2*rod_gap) return {human, two_bar};

diff --git a/software/algo.hpp b/software/algo.hpp
@@ -22,6 +22,16 @@ typedef enum state_t {
     num_state_t
 } state_t;
 
+typedef enum control_task_t {
+    control_task_init,
+    control_task_raise,
+    control_task_move_lateral,
+    control_task_lower,
+    control_task_push,
+    control_task_shoot,
+    control_task_adjust,
+} control_task_t;
+
 struct motor_cmd {
     // NAN for unchanged
     double pos;
@@ -43,7 +53,20 @@ state_t algo_action(
         vector<motor_cmd> (&mtr_cmds)[num_axis_t]
 );
 
+/**
+ * Gets index of player currently closest to target_cm
+ */
 int closest_plr(int rod, double target_cm, double cur_pos);
 
+/**
+ * Gets x offset of a player on a specific rod
+ */
+double plr_offset(int plr, int rod);
+
+/**
+ * Gets rod closest to ball_cm
+ * ret[0]: side_t
+ * ret[1]: rod_t
+ */
 pair<side_t, rod_t> closest_rod(double ball_cm);