-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathalgo.cpp
122 lines (97 loc) · 4.2 KB
/
algo.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
#include "algo.hpp"
#include "physical_params.hpp"
#include <algorithm>
#include <iostream>
using namespace std;
/******************************************************************************
* Defines
******************************************************************************/
// Pretty ugly hack to avoid rewriting the common parameters a bunch
// I could wrap them in a struct or something but seems kinda pointless
#define params \
state_t cur_state,\
vector<double> ball_pos,\
vector<double> ball_vel,\
vector<motor_cmd> (&mtr_cmds)[num_axis_t]
/******************************************************************************
* Private functions
******************************************************************************/
state_t algo_defense(params){
return state_defense;
}
// Used for checking for blocking
int closest_plr_ignore_walls(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;
int plr2 = plr1_pos > target_cm ? plr1-1 : plr1+1;
if(plr2 >= num_plrs[rod] || plr2 < 0) return plr1;
double plr2_rest = bumper_width + plr_width/2 + plr2*plr_gap[rod];
double plr2_pos = plr2_rest + cur_pos;
if(abs(plr2_pos-target_cm) < abs(plr1_pos-target_cm))
return plr2;
else
return plr1;
}
/******************************************************************************
* Public functions
******************************************************************************/
state_t (*algo_fns[num_state_t])(params) = {
[state_defense]=algo_defense,
};
state_t algo_action(params){
return algo_fns[cur_state](cur_state, ball_pos, ball_vel, mtr_cmds);
}
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;
int plr2 = plr1_pos > target_cm ? plr1-1 : plr1+1;
if(plr2 >= num_plrs[rod] || plr2 < 0) return plr1;
double plr2_rest = bumper_width + plr_width/2 + plr2*plr_gap[rod];
double plr2_pos = plr2_rest + cur_pos;
if(abs(plr2_pos-target_cm) < abs(plr1_pos-target_cm)
&& target_cm > plr2_rest && target_cm < plr2_rest+lin_range_cm[rod])
return plr2;
else
return plr1;
}
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};
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};
}
bool is_blocked(int start_rod, double ball_cm, double rod_pos[num_axis_t][num_rod_t], double tol, int end_rod){
if((ball_cm < play_height/2 - goal_width/2 || ball_cm > play_height/2 + goal_width/2) && end_rod < 0)
return true;
int r = goalie;
while(-rod_coord[r] > rod_coord[start_rod]){
if(end_rod >= 0 && -rod_coord[r] > rod_coord[end_rod]){
--r;
continue;
}
int plr = closest_plr_ignore_walls(r, ball_cm, rod_pos[lin][r]);
if(abs(ball_cm - (plr_offset(plr, r) + rod_pos[lin][r])) < ball_rad + foot_width/2 + tol) return true;
/* cout << r << ", " << plr_offset(plr, r) << ", " << rod_pos[lin][r] << endl; */
--r;
}
return false;
}
double kin_ball_dist(vector<double> ball_pos, vector<double> ball_vel, double y){
double dt = (y - ball_pos[1])/ball_vel[1];
double x = ball_pos[0] + ball_vel[0]*dt;
int num_bounces = floor(x/play_height);
bool flipped = num_bounces%2 == 0;
x = fmod(x, play_height);
return flipped ? play_height - x : x;
}
bool can_plr_reach(int plr, int rod, double target_cm, double tol){
double offset = plr_offset(plr, rod);
return target_cm >= offset-tol && target_cm <= offset+lin_range_cm[rod]+tol;
}