Skip to content

Commit 4e6ecad

Browse files
SarangaRSarangaR
committed
tuning
1 parent 66231d2 commit 4e6ecad

7 files changed

Lines changed: 730 additions & 0 deletions

File tree

‎night_config.toml‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
control_board_path = "/dev/ttyACM0"
2+
control_board_backup_path = "/dev/ttyACM4"
3+
meb_path = "/dev/ttyACM2"
4+
front_cam_path = "/dev/video0"
5+
bottom_cam_path = "/dev/video1"
6+
color_profile = "Night Testing"
7+
shark = "Left"
8+
saw_fish = "Right"
9+
10+
[missions.gate]
11+
depth = -1.0
12+
speed = 1.0
13+
true_count = 4
14+
false_count = 1
15+
side = "Right"
16+
17+
[missions.path_align]
18+
depth = -1.0
19+
speed = -0.0
20+
forward_speed = 0.2
21+
detections = 8
22+
23+
[missions.slalom]
24+
depth = -0.75
25+
speed = 0.4
26+
start_detections = 5
27+
end_detections = 10
28+
side = "Right"
29+
centered_threshold = 0.0
30+
dumb_strafe_secs = 1
31+
init_duration = 4.0
32+
strafe_duration = 2.0
33+
traversal_duration = 6.0
34+
yaw_adjustment = 20.0
35+
yaw_speed = -0.2
36+
area_bounds = { start = 630.0, end = 11000.0 }
37+
correction_factor = -0.4
38+
39+
40+
[missions.coinflip]
41+
depth = -1.15
42+
angle_correction = 0.2
43+
true_count = 2
44+
45+
[missions.octagon]
46+
47+
[missions.bin]
48+
depth = -1.0
49+
speed = 0.3
50+
51+
[sonar]
52+
serial_port = "/dev/ttyUSB0"
53+
serial_baud_rate = 115200
54+
bootloader = "Skip"
55+
[sonar.auto_transmit]
56+
mode = 1
57+
gain_setting = "Low"
58+
transmit_duration = 80
59+
sample_period = 500
60+
transmit_frequency = 750
61+
number_of_samples = 1024
62+
start_angle = 0
63+
stop_angle = 399
64+
num_steps = 1
65+
delay = 0
66+
67+
[color_profiles."Night Testing".red]
68+
start = { y = 107, u = 92, v = 100 }
69+
end = { y = 167, u = 160, v = 128 }
70+
71+
[color_profiles."Night Testing".orange]
72+
start = { y = 73, u = 87, v = 137 }
73+
end = { y = 189, u = 136, v = 222 }
74+
75+
[color_profiles."Night Testing".purple]
76+
start = { y = 0, u = 131, v = 117 }
77+
end = { y = 139, u = 255, v = 255 }
78+
79+
[color_profiles."Night Testing".yellow]
80+
start = { y = 0, u = 0, v = 111 }
81+
end = { y = 144, u = 114, v = 255 }
82+
83+
[color_profiles."B2 Day".red]
84+
start = { y = 77, u = 101, v = 113 }
85+
end = { y = 200, u = 170, v = 210 }
86+
87+
[color_profiles."B2 Day".orange]
88+
start = { y = 111, u = 55, v = 134 }
89+
end = { y = 255, u = 112, v = 194 }
90+
91+
[color_profiles."B2 Day".purple]
92+
start = { y = 0, u = 131, v = 117 }
93+
end = { y = 139, u = 255, v = 255 }
94+
95+
[color_profiles."B2 Day".yellow]
96+
start = { y = 0, u = 0, v = 111 }
97+
end = { y = 144, u = 114, v = 255 }
98+
99+

‎src/config/mod.rs‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,7 @@ pub struct ColorProfile {
8686
pub orange: RangeInclusive<Yuv>,
8787
pub yellow: RangeInclusive<Yuv>,
8888
pub purple: RangeInclusive<Yuv>,
89+
pub black: RangeInclusive<Yuv>,
8990
}
9091

9192
#[derive(Debug, Serialize, Deserialize, PartialEq, Eq, Hash, Clone)]

‎src/missions/gate.rs‎

Lines changed: 203 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@ use crate::{
1515
vision::{MidPoint, OffsetClass},
1616
},
1717
vision::{
18+
gate_cv::GateCV,
1819
gate_poles::{GatePoles, Target},
1920
nn_cv2::{OnnxModel, YoloClass},
2021
Offset2D,
@@ -37,6 +38,208 @@ use super::{
3738
vision::{DetectTarget, ExtractPosition, VisionNorm, VisionNormOffset},
3839
};
3940

41+
pub async fn gate_run_cv_procedural<
42+
Con: Send + Sync + GetControlBoard<WriteHalf<SerialStream>> + GetMainElectronicsBoard + FrontCamIO,
43+
>(
44+
context: &Con,
45+
config: &Config,
46+
) {
47+
#[cfg(feature = "logging")]
48+
logln!("Starting Procedural Gate");
49+
50+
let cb = context.get_control_board();
51+
let _ = cb.bno055_periodic_read(true).await;
52+
53+
let mut vision =
54+
VisionNorm::<Con, GatePoles<OnnxModel>, f64>::new(context, GatePoles::default());
55+
56+
let initial_yaw = loop {
57+
if let Some(initial_angle) = cb.responses().get_angles().await {
58+
break *initial_angle.yaw();
59+
} else {
60+
#[cfg(feature = "logging")]
61+
logln!("Failed to get initial angle");
62+
}
63+
};
64+
65+
let _ = cb
66+
.stability_2_speed_set(0.0, 0.0, 0.0, 0.0, initial_yaw, config.depth)
67+
.await;
68+
69+
const TOLERANCE: f32 = 0.3;
70+
71+
let mut true_count = 0;
72+
73+
loop {
74+
#[allow(unused_variables)]
75+
let detections = vision.execute().await.unwrap_or_else(|e| {
76+
#[cfg(feature = "logging")]
77+
logln!("Getting path detection resulted in error: `{e}`\n\tUsing empty detection vec");
78+
vec![]
79+
});
80+
81+
let rightPole = detections
82+
.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+
91+
let shark = detections
92+
.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;
116+
117+
let _ = cb
118+
.stability_2_speed_set(
119+
correction,
120+
fwd,
121+
0.0,
122+
0.0,
123+
initial_yaw,
124+
config.depth,
125+
)
126+
.await;
127+
} else {
128+
let fwd = config.speed;
129+
let correction = 0.05;
130+
true_count += 1;
131+
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;
148+
break;
149+
}
150+
}
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;
167+
}
168+
}
169+
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;
185+
186+
let _ = cb
187+
.stability_2_speed_set(
188+
correction,
189+
fwd,
190+
0.0,
191+
0.0,
192+
initial_yaw,
193+
config.depth,
194+
)
195+
.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;
203+
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;
220+
break;
221+
}
222+
}
223+
} else {
224+
// Fallback search behavior
225+
#[cfg(feature = "logging")]
226+
logln!("RIGHT: Missing Features, Fallback");
227+
228+
let correction = 0.2;
229+
let fwd = 0.05;
230+
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+
}
238+
}
239+
}
240+
}
241+
}
242+
40243
pub async fn gate_run_procedural<
41244
Con: Send + Sync + GetControlBoard<WriteHalf<SerialStream>> + GetMainElectronicsBoard + FrontCamIO,
42245
>(

0 commit comments

Comments
 (0)