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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
use robstride::{
robstride02::RobStride02, ActuatorConfiguration, ActuatorType, ControlConfig, StubTransport,
Supervisor, TransportType,
};
use std::time::Duration;
use tracing::{debug, error, info};
use tracing_subscriber::EnvFilter;
#[tokio::main]
async fn main() -> eyre::Result<()> {
// Initialize tracing with specific filters
tracing_subscriber::fmt()
.with_env_filter(
EnvFilter::from_default_env()
.add_directive("polling=off".parse().unwrap())
.add_directive("async_io=off".parse().unwrap()),
)
.init();
let mut supervisor = Supervisor::new(Duration::from_millis(1000))?;
// Initialize SocketCAN transport
// let socketcan = SocketCanTransport::new("can0".to_string()).await?;
// supervisor
// .add_transport("socketcan".to_string(), TransportType::SocketCAN(socketcan))
// .await?;
let stub = StubTransport::new("stub".to_string());
supervisor
.add_transport("stub".to_string(), TransportType::Stub(stub))
.await?;
// Spawn supervisor task
let mut supervisor_runner = supervisor.clone_controller();
let supervisor_handle = tokio::spawn(async move {
info!("Starting supervisor task");
if let Err(e) = supervisor_runner.run(Duration::from_millis(500)).await {
error!("Supervisor task failed: {}", e);
}
});
// // Scan for actuators on the bus
// let desired_actuator_types: &[(u8, ActuatorConfiguration)] = &[
// (1, ActuatorConfiguration {
// actuator_type: ActuatorType::RobStride04,
// ..Default::default()
// }),
// ];
// let discovered_ids = supervisor
// .scan_bus(0xFD, "socketcan", &desired_actuator_types)
// .await?;
// debug!("Discovered IDs: {:?}", discovered_ids);
supervisor
.add_actuator(
Box::new(RobStride02::new(
1,
0xFE,
supervisor.get_transport_tx("stub").await?,
)),
ActuatorConfiguration {
actuator_type: ActuatorType::RobStride02,
..Default::default()
},
)
.await;
// Configure actuator 1
let config = ControlConfig {
kp: 5.0,
kd: 0.1,
max_torque: Some(2.0),
max_velocity: Some(1.0),
max_current: Some(10.0),
};
// for id in &discovered_ids {
// supervisor.configure(*id, config.clone()).await?;
// // supervisor.zero(*id).await?;
// }
// Enable actuator 1 AFTER spawning the supervisor
// debug!("Enabling actuator 1");
// supervisor.enable(1).await?;
// Start control loop with original supervisor instance
// let mut target_angle = std::f32::consts::FRAC_PI_2;
// Create a ctrl-c handler
let (tx, mut rx) = tokio::sync::mpsc::channel(1);
let tx_clone = tx.clone();
tokio::spawn(async move {
if let Err(e) = tokio::signal::ctrl_c().await {
error!("Failed to listen for ctrl-c: {}", e);
}
let _ = tx_clone.send(()).await;
});
info!("Configuring actuator 1");
supervisor.configure(1, config.clone()).await?;
supervisor.enable(1).await?;
info!("Starting control loop, press Ctrl+C to exit");
loop {
tokio::select! {
_ = rx.recv() => {
info!("Received shutdown signal");
break;
}
_ = tokio::time::sleep(Duration::from_millis(500)) => {
supervisor.command(1, 1.0, 0.0, 0.0).await?;
// for id in &discovered_ids {
// let feedback = supervisor.get_feedback(*id).await?;
// if let Some((feedback, _ts)) = feedback {
// debug!("Motor {}: angle = {:.3} rad ({:.1}°)",
// id,
// feedback.angle,
// feedback.angle.to_degrees()
// );
// }
// }
}
}
}
// Clean shutdown
info!("Shutting down supervisor...");
supervisor_handle.abort();
// for id in &discovered_ids {
// if let Err(e) = supervisor.disable(*id, false).await {
// error!("Failed to disable actuator {}: {}", id, e);
// }
// debug!("Disabled actuator {}", id);
// }
Ok(())
}