More consistent defense

misprit7 · Jan 22, 2024 · b55bc7f · b55bc7f
1 parent 928226a
commit b55bc7f
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 9 deletions.
diff --git a/software/algo.cpp b/software/algo.cpp
@@ -39,6 +39,9 @@ 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;
@@ -54,7 +57,22 @@ int closest_plr(int rod, double target_cm, double cur_pos){
         return plr2;
     else
         return plr1;
-
+}
+
+/*
+ * Gets rod closest to ball_cm
+ * ret[0]: side_t
+ * ret[1]: rod_t
+ */
+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};
+    if(ball_cm >= rod_gap) return {bot, three_bar};
+    if(ball_cm >= 0) return {human, five_bar};
+    if(ball_cm >= -rod_gap) return {bot, five_bar};
+    if(ball_cm >= -2*rod_gap) return {human, three_bar};
+    if(ball_cm >= -3*rod_gap) return {bot, two_bar};
+    return {bot, goalie};
 }
 
 

diff --git a/software/algo.hpp b/software/algo.hpp
@@ -15,10 +15,9 @@ typedef enum state_t {
     state_defense,
     state_shot_defense,
     state_shot_offense,
-    state_goalie_possess,
-    state_two_bar_possess,
-    state_five_bar_possess,
-    state_three_bar_possess,
+    state_uncontrolled,
+    state_controlled,
+    state_shooting,
     state_unknown,
     num_state_t
 } state_t;
@@ -46,5 +45,5 @@ state_t algo_action(
 
 int closest_plr(int rod, double target_cm, double cur_pos);
 
-
+pair<side_t, rod_t> closest_rod(double ball_cm);
 
diff --git a/software/main.cpp b/software/main.cpp
@@ -687,9 +687,14 @@ int main(int argc, char** argv){
             case state_defense:
             {
                 int front;
-                if(ball_pos[1] > 2*rod_gap) front = three_bar;
-                else if(ball_pos[1] > 0) front = five_bar;
-                else if(ball_pos[1] > -2*rod_gap) front = two_bar;
+                pair<side_t, rod_t> closest = closest_rod(ball_pos[1]);
+                if(closest.first == bot){
+                    state = state_uncontrolled;
+                    break;
+                }
+                if(closest.second == three_bar) front = two_bar;
+                else if(closest.second == five_bar) front = five_bar;
+                else front = three_bar;
 
                 /* ball_vel = {20,-200,0}; */
                 double cooldown_time = 25;
@@ -801,6 +806,7 @@ int main(int argc, char** argv){
                 if(ball_vel[1] > -50) state = state_defense;
                 break;
             }
+            case state_uncontrolled:
             case state_unknown:
             default:
                 break;

diff --git a/software/physical_params.hpp b/software/physical_params.hpp
@@ -30,6 +30,11 @@ typedef enum axis_t {
     num_axis_t
 } axis_t;
 
+typedef enum side_t {
+    bot,
+    human,
+    num_side_t
+} side_t;
 
 /******************************************************************************
  * Table Dimensions