Skip to content

Commit ed32f56

Browse files
CowboylaserkittenjetsharkCowboylaserkittenjetshark
committed
gate cv maybe
1 parent 43de7a8 commit ed32f56

6 files changed

Lines changed: 200 additions & 182 deletions

File tree

‎src/config/gate.rs‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,13 @@ pub struct Config {
88
pub true_count: u32,
99
pub false_count: u32,
1010
pub side: Side,
11+
pub yaw_speed: f32,
12+
pub strafe_speed: f32,
13+
pub init_duration: f32,
14+
pub strafe_duration: f32,
15+
pub traversal_duration: f32,
16+
pub yaw_adjustment: f32,
17+
pub correction_factor: f32,
1118
}
1219

1320
impl Default for Config {
@@ -18,6 +25,13 @@ impl Default for Config {
1825
true_count: 4,
1926
false_count: 1,
2027
side: Side::default(),
28+
yaw_speed: 0.2,
29+
strafe_speed: 0.2,
30+
correction_factor: 0.2,
31+
init_duration: 3.0,
32+
strafe_duration: 2.0,
33+
traversal_duration: 8.0,
34+
yaw_adjustment: 20.0,
2135
}
2236
}
2337
}

‎src/main.rs‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,10 @@ use sw8s_rust_lib::{
2323
coinflip::{coinflip, coinflip_procedural},
2424
example::{initial_descent, pid_test},
2525
fire_torpedo::{FireLeftTorpedo, FireRightTorpedo},
26-
gate::{gate_run_complex, gate_run_naive, gate_run_procedural, gate_run_testing},
26+
gate::{
27+
gate_run_complex, gate_run_cv_procedural, gate_run_naive, gate_run_procedural,
28+
gate_run_testing,
29+
},
2730
meb::WaitArm,
2831
octagon::octagon,
2932
path_align::{path_align_procedural, static_align_procedural},
@@ -385,7 +388,7 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> {
385388
gate_target().await,
386389
))
387390
.execute()),
388-
"gate_run_coinflip" => ctwrap!(gate_run_procedural(
391+
"gate_run_coinflip" => ctwrap!(gate_run_cv_procedural(
389392
&FullActionContext::new(
390393
control_board().await,
391394
meb().await,
@@ -394,6 +397,7 @@ async fn run_mission(mission: &str, cancel: CancellationToken) -> Result<()> {
394397
gate_target().await,
395398
),
396399
&config.missions.gate,
400+
&config.get_color_profile().unwrap(),
397401
)),
398402
"gate_run_testing" => ctwrap!(gate_run_testing(&FullActionContext::new(
399403
control_board().await,

‎src/missions/gate.rs‎

Lines changed: 140 additions & 129 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,11 @@
11
use itertools::Itertools;
22
use tokio::io::WriteHalf;
3+
use tokio::time::{sleep, Duration};
34
use tokio_serial::SerialStream;
45

56
use crate::{
67
act_nest,
7-
config::{gate::Config, Side},
8+
config::{gate::Config, ColorProfile, Side},
89
missions::{
910
action::{ActionConcurrentSplit, ActionDataConditional},
1011
basic::descend_depth_and_go_forward,
@@ -43,15 +44,17 @@ pub async fn gate_run_cv_procedural<
4344
>(
4445
context: &Con,
4546
config: &Config,
47+
color_profile: &ColorProfile,
4648
) {
4749
#[cfg(feature = "logging")]
4850
logln!("Starting Procedural Gate");
4951

5052
let cb = context.get_control_board();
5153
let _ = cb.bno055_periodic_read(true).await;
5254

55+
// let mut vision = VisionNorm::<Con, GatePoles<OnnxModel>, f64>::new(context, GateCV::default());
5356
let mut vision =
54-
VisionNorm::<Con, GatePoles<OnnxModel>, f64>::new(context, GatePoles::default());
57+
VisionNorm::<Con, GateCV, f64>::new(context, GateCV::from_color_profile(color_profile));
5558

5659
let initial_yaw = loop {
5760
if let Some(initial_angle) = cb.responses().get_angles().await {
@@ -68,7 +71,10 @@ pub async fn gate_run_cv_procedural<
6871

6972
const TOLERANCE: f32 = 0.3;
7073

74+
let mut gate_state = GateState::Align;
75+
let mut yaw_target = 0.0;
7176
let mut true_count = 0;
77+
let mut false_count = 0;
7278

7379
loop {
7480
#[allow(unused_variables)]
@@ -78,163 +84,163 @@ pub async fn gate_run_cv_procedural<
7884
vec![]
7985
});
8086

81-
let rightPole = detections
87+
let leftPole = detections.iter().filter(|d| *d.class()).collect_vec();
88+
let leftPole_avg_x = leftPole
8289
.iter()
83-
.filter(|d| matches!(d.class().identifier, Target::RightPole))
84-
.collect_vec();
85-
86-
/* let middle = detections
87-
.iter()
88-
.filter(|d| matches!(d.class().identifier, Target::Middle))
89-
.collect_vec(); */
90+
.map(|d| *d.position().x() as f32)
91+
.sum::<f32>();
9092

91-
let shark = detections
93+
let rightPole = detections.iter().filter(|d| !*d.class()).collect_vec();
94+
let rightPole_avg_x = rightPole
9295
.iter()
93-
.filter(|d| matches!(d.class().identifier, Target::Sawfish))
94-
.collect_vec();
95-
96-
let sawfish = detections
97-
.iter()
98-
.filter(|d| matches!(d.class().identifier, Target::Shark))
99-
.collect_vec();
100-
101-
let mut traversal_timer = DelayAction::new(8.0); // forward duration in second
102-
103-
match config.side {
104-
Side::Left => {
105-
if !shark.is_empty() {
106-
// Center on average x of blue
107-
let avg_x = shark.iter().map(|d| *d.position().x() as f32).sum::<f32>()
108-
/ shark.len() as f32;
109-
110-
#[cfg(feature = "logging")]
111-
logln!("SHARK AVG X: {}", avg_x);
112-
113-
if avg_x.abs() > TOLERANCE {
114-
let correction = 0.4 * avg_x;
115-
let fwd = 0.0;
96+
.map(|d| *d.position().x() as f32)
97+
.sum::<f32>();
98+
99+
match gate_state {
100+
GateState::Align => match config.side {
101+
Side::Left => {
102+
if leftPole.len() > 0 {
103+
false_count = 0;
104+
let mut correction;
105+
if leftPole_avg_x < 0.2 {
106+
true_count += 1;
107+
if true_count >= config.true_count {
108+
#[cfg(feature = "logging")]
109+
logln!("ALIGNED");
110+
if let Some(current_angle) = cb.responses().get_angles().await {
111+
let current_yaw = *current_angle.yaw();
112+
yaw_target = current_yaw;
113+
}
114+
gate_state = GateState::Approach;
115+
} else {
116+
#[cfg(feature = "logging")]
117+
logln!("true_count: {true_count}/4");
118+
}
119+
} else {
120+
correction = dbg!(config.correction_factor * leftPole_avg_x);
121+
let _ = cb
122+
.stability_1_speed_set(0.0, 0.0, correction, 0.0, 0.0, config.depth)
123+
.await;
124+
}
125+
} else {
126+
#[cfg(feature = "logging")]
127+
logln!("SEARCHING");
116128

117129
let _ = cb
118-
.stability_2_speed_set(
119-
correction,
120-
fwd,
130+
.stability_1_speed_set(
131+
0.0,
132+
0.0,
133+
-config.yaw_speed,
121134
0.0,
122135
0.0,
123-
initial_yaw,
124136
config.depth,
125137
)
126138
.await;
127-
} else {
128-
let fwd = config.speed;
129-
let correction = 0.05;
130-
true_count += 1;
131139

132-
if true_count >= config.true_count {
133-
let _ = cb
134-
.stability_2_speed_set(
135-
correction,
136-
fwd,
137-
0.0,
138-
0.0,
139-
initial_yaw,
140-
config.depth,
141-
)
142-
.await;
143-
// let _ = cb
144-
// .stability_1_speed_set(correction, fwd, 0.0, 0.0, 0.0, config.depth)
145-
// .await;
146-
147-
traversal_timer.execute().await;
140+
false_count += 1;
141+
#[cfg(feature = "logging")]
142+
logln!("NO DETECTIONS");
143+
if false_count >= 100 {
144+
#[cfg(feature = "logging")]
145+
logln!("KILLED NO DET");
148146
break;
149147
}
150148
}
151-
} else {
152-
// Fallback search behavior
153-
#[cfg(feature = "logging")]
154-
logln!("LEFT: Missing Features, Fallback");
155-
156-
let correction = -0.2;
157-
let fwd = 0.05;
158-
159-
let _ = cb
160-
.stability_2_speed_set(correction, fwd, 0.0, 0.0, initial_yaw, config.depth)
161-
.await;
162-
// let _ = cb
163-
// .stability_1_speed_set(correction, fwd, 0.0, 0.0, 0.0, config.depth)
164-
// .await;
165-
166-
DelayAction::new(1.0).execute().await;
167149
}
168-
}
169150

170-
Side::Right => {
171-
if !sawfish.is_empty() {
172-
// Center on average x of blue
173-
let avg_x = (sawfish
174-
.iter()
175-
.map(|d| *d.position().x() as f32)
176-
.sum::<f32>()
177-
/ sawfish.len() as f32);
178-
179-
#[cfg(feature = "logging")]
180-
logln!("SAWFISH AVG X: {}", avg_x);
181-
182-
if avg_x.abs() > TOLERANCE {
183-
let correction = 0.4 * avg_x;
184-
let fwd = 0.05;
151+
Side::Right => {
152+
if rightPole.len() > 0 {
153+
false_count = 0;
154+
let mut correction;
155+
if rightPole_avg_x < 0.2 {
156+
true_count += 1;
157+
if true_count >= config.true_count {
158+
#[cfg(feature = "logging")]
159+
logln!("ALIGNED");
160+
if let Some(current_angle) = cb.responses().get_angles().await {
161+
let current_yaw = *current_angle.yaw();
162+
yaw_target = current_yaw;
163+
}
164+
gate_state = GateState::Approach;
165+
} else {
166+
#[cfg(feature = "logging")]
167+
logln!("true_count: {true_count}/4");
168+
}
169+
} else {
170+
correction = dbg!(config.correction_factor * rightPole_avg_x);
171+
let _ = cb
172+
.stability_1_speed_set(0.0, 0.0, correction, 0.0, 0.0, config.depth)
173+
.await;
174+
}
175+
} else {
176+
#[cfg(feature = "logging")]
177+
logln!("SEARCHING");
185178

186179
let _ = cb
187-
.stability_2_speed_set(
188-
correction,
189-
fwd,
180+
.stability_1_speed_set(
181+
0.0,
182+
0.0,
183+
config.yaw_speed,
190184
0.0,
191185
0.0,
192-
initial_yaw,
193186
config.depth,
194187
)
195188
.await;
196-
// let _ = cb
197-
// .stability_1_speed_set(correction, fwd, 0.0, 0.0, 0.0, config.depth)
198-
// .await;
199-
} else {
200-
let fwd = config.speed;
201-
let correction = 0.05;
202-
true_count += 1;
203189

204-
if true_count >= config.true_count {
205-
let _ = cb
206-
.stability_2_speed_set(
207-
correction,
208-
fwd,
209-
0.0,
210-
0.0,
211-
initial_yaw,
212-
config.depth,
213-
)
214-
.await;
215-
// let _ = cb
216-
// .stability_1_speed_set(correction, fwd, 0.0, 0.0, 0.0, config.depth)
217-
// .await;
218-
219-
traversal_timer.execute().await;
190+
false_count += 1;
191+
#[cfg(feature = "logging")]
192+
logln!("NO DETECTIONS");
193+
if false_count >= 100 {
194+
#[cfg(feature = "logging")]
195+
logln!("KILLED NO DET");
220196
break;
221197
}
222198
}
199+
}
200+
},
201+
GateState::Approach => {
202+
#[cfg(feature = "logging")]
203+
logln!("APPROACH");
204+
205+
let strafe_direction = if let Side::Left = config.side {
206+
-1.0
223207
} else {
224-
// Fallback search behavior
225-
#[cfg(feature = "logging")]
226-
logln!("RIGHT: Missing Features, Fallback");
208+
1.0
209+
};
210+
211+
let _ = cb
212+
.stability_2_speed_set(
213+
config.strafe_speed * strafe_direction,
214+
0.0,
215+
0.0,
216+
0.0,
217+
yaw_target,
218+
config.depth,
219+
)
220+
.await;
221+
222+
sleep(Duration::from_secs(config.strafe_duration as u64)).await;
223+
224+
yaw_target = (yaw_target
225+
+ (if let Side::Left = config.side {
226+
config.yaw_adjustment
227+
} else {
228+
-config.yaw_adjustment
229+
}));
227230

228-
let correction = 0.2;
229-
let fwd = 0.05;
231+
let _ = cb
232+
.stability_2_speed_set(0.0, 0.0, 0.0, 0.0, yaw_target, config.depth)
233+
.await;
230234

231-
let _ = cb
232-
.stability_2_speed_set(correction, fwd, 0.0, 0.0, initial_yaw, config.depth)
233-
.await;
234-
// let _ = cb
235-
// .stability_1_speed_set(correction, fwd, 0.0, 0.0, 0.0, config.depth)
236-
// .await;
237-
}
235+
sleep(Duration::from_secs(config.init_duration as u64)).await;
236+
237+
let _ = cb
238+
.stability_2_speed_set(0.0, config.speed, 0.0, 0.0, yaw_target, config.depth)
239+
.await;
240+
241+
sleep(Duration::from_secs(config.traversal_duration as u64)).await;
242+
243+
break;
238244
}
239245
}
240246
}
@@ -671,3 +677,8 @@ pub fn gate_run_testing<
671677
let depth: f32 = -1.0;
672678
adjust_logic(context, depth, CountTrue::new(3))
673679
}
680+
681+
enum GateState {
682+
Align,
683+
Approach,
684+
}

0 commit comments

Comments
 (0)