|
| 1 | +use std::{ |
| 2 | + sync::Arc, |
| 3 | + time::Duration, |
| 4 | + error::Error, |
| 5 | +// default::Default, |
| 6 | + }; |
| 7 | +use etherage::{ |
| 8 | + EthernetSocket, |
| 9 | + SlaveAddress, |
| 10 | + CommunicationState, Master, |
| 11 | + registers, |
| 12 | + mapping, |
| 13 | + sdo, |
| 14 | + }; |
| 15 | +use ioprio::*; |
| 16 | +use futures::stream::StreamExt; |
| 17 | +use futures_concurrency::future::{Join, Race}; |
| 18 | + |
| 19 | + |
| 20 | +#[tokio::main] |
| 21 | +async fn main() -> Result<(), Box<dyn Error>> { |
| 22 | + // RT this thread and all child threads |
| 23 | + thread_priority::set_current_thread_priority(thread_priority::ThreadPriority::Max).unwrap(); |
| 24 | + #[cfg(target_os = "linux")] |
| 25 | + ioprio::set_priority( |
| 26 | + ioprio::Target::Process(ioprio::Pid::this()), |
| 27 | + Priority::new(ioprio::Class::Realtime(ioprio::RtPriorityLevel::highest())), |
| 28 | + ).unwrap(); |
| 29 | + |
| 30 | + //Init master |
| 31 | + let master = Arc::new(Master::new(EthernetSocket::new("eno1")?)); |
| 32 | + |
| 33 | + master.switch(CommunicationState::Init).await.unwrap(); |
| 34 | + let raw = unsafe {master.get_raw()}; |
| 35 | + ( |
| 36 | + master.reset_addresses(), |
| 37 | + master.reset_clock(), |
| 38 | + master.reset_logical(), |
| 39 | + master.reset_mailboxes(), |
| 40 | + raw.bwr(registers::ports_errors, Default::default()), |
| 41 | + ).join().await; |
| 42 | + |
| 43 | + let mut tasks = Vec::new(); |
| 44 | + let mut iter = master.discover().await; |
| 45 | + while let Some(mut slave) = iter.next().await { |
| 46 | + tasks.push(async move { |
| 47 | + let SlaveAddress::AutoIncremented(i) = slave.address() |
| 48 | + else { panic!("slave already has a fixed address") }; |
| 49 | + slave.expect(etherage::CommunicationState::Init); |
| 50 | + slave.set_address(i+1).await.unwrap(); |
| 51 | + slave.init_mailbox().await.unwrap(); |
| 52 | + slave.init_coe().await; |
| 53 | + |
| 54 | + slave |
| 55 | + }); |
| 56 | + } |
| 57 | + let mut slaves = tasks.join().await; |
| 58 | + |
| 59 | + |
| 60 | +// println!("custom init"); |
| 61 | +// for slave in &slaves { |
| 62 | +// // let mut coe = slave.coe().await; |
| 63 | +// // let raw = unsafe {slave.raw_master()}; |
| 64 | +// // let priority = bilge::prelude::u2::new(0); |
| 65 | +// |
| 66 | +// raw.write(slave.address(), registers::isochronous::sync::enable, Default::default()).await.one().unwrap(); |
| 67 | +// raw.write(slave.address(), etherage::Field::<u8>::simple(0x980), 0).await.one().unwrap(); |
| 68 | +// let period = 2_000_000; |
| 69 | +// let start = raw.read(slave.address(), registers::dc::system_time).await.one().unwrap() as u32; |
| 70 | +// let start = (start / period) * period + period; |
| 71 | +// raw.write(slave.address(), registers::isochronous::sync::start_time, start).await.one().unwrap(); |
| 72 | +// raw.write(slave.address(), registers::isochronous::sync::sync0_cycle_time, period).await.one().unwrap(); |
| 73 | +// raw.write(slave.address(), registers::isochronous::sync::enable, { |
| 74 | +// let mut enables = registers::IsochronousEnables::default(); |
| 75 | +// enables.set_operation(true); |
| 76 | +// enables.set_sync0(true); |
| 77 | +// enables |
| 78 | +// }).await.one().unwrap(); |
| 79 | +// |
| 80 | +// // let period = 2_000_000; |
| 81 | +// // let start = raw.read(slave.address(), registers::dc::system_time).await.one().unwrap() as u32; |
| 82 | +// // let start = (start / period) * period + period; |
| 83 | +// // // let start = 2_001_000; |
| 84 | +// // raw.write(slave.address(), registers::isochronous::all, { |
| 85 | +// // let mut isochronous = registers::Isochronous::default(); |
| 86 | +// // isochronous.enable.set_operation(true); |
| 87 | +// // isochronous.enable.set_sync0(true); |
| 88 | +// // isochronous.sync0_cycle_time = period; |
| 89 | +// // isochronous.start_time = start; |
| 90 | +// // isochronous |
| 91 | +// // }).await.one().unwrap(); |
| 92 | +// // coe.sdo_write(&sdo::sync_manager.logical_write().sync().sync_mode(), priority, sdo::SyncMode::DCSync0).await.unwrap(); |
| 93 | +// } |
| 94 | + |
| 95 | + // initialize clocks and perform static drift |
| 96 | + master.init_clock().await.unwrap(); |
| 97 | + |
| 98 | + // create a mapping buffering all divergences |
| 99 | + let config = mapping::Config::default(); |
| 100 | + let mapping = mapping::Mapping::new(&config); |
| 101 | + let mut offsets = Vec::new(); |
| 102 | + for slave in &slaves { |
| 103 | + let SlaveAddress::Fixed(i) = slave.address() |
| 104 | + else { panic!("slave has no fixed address") }; |
| 105 | + let mut slave = mapping.slave(i); |
| 106 | + offsets.push(slave.register(sdo::SyncDirection::Read, registers::dc::system_time)); |
| 107 | + slave.channel(sdo::sync_manager.logical_read()); |
| 108 | + slave.channel(sdo::sync_manager.logical_write()); |
| 109 | + } |
| 110 | + let group = master.group(&mapping); |
| 111 | + |
| 112 | + |
| 113 | + |
| 114 | + master.switch(CommunicationState::PreOperational).await.unwrap(); |
| 115 | + |
| 116 | +// for slave in &slaves { |
| 117 | +// dbg!(slave.coe().await.sdo_read(&sdo::sync_manager.channel(3).sync().supported_modes(), Default::default()).await); |
| 118 | +// } |
| 119 | + |
| 120 | + for slave in &mut slaves { |
| 121 | + slave.expect(CommunicationState::PreOperational); |
| 122 | + group.configure(&slave).await.unwrap(); |
| 123 | + } |
| 124 | + |
| 125 | + |
| 126 | + println!("custom init"); |
| 127 | + for slave in &slaves { |
| 128 | + |
| 129 | + raw.write(slave.address(), registers::isochronous::sync::enable, Default::default()).await.one().unwrap(); |
| 130 | + raw.write(slave.address(), etherage::Field::<u8>::simple(0x980), 0).await.one().unwrap(); |
| 131 | + let period = 2_000_000; |
| 132 | + let start = raw.read(slave.address(), registers::dc::system_time).await.one().unwrap() as u32; |
| 133 | + let start = (start / period) * period + period; |
| 134 | + raw.write(slave.address(), registers::isochronous::sync::start_time, start).await.one().unwrap(); |
| 135 | + raw.write(slave.address(), registers::isochronous::sync::sync0_cycle_time, period).await.one().unwrap(); |
| 136 | + raw.write(slave.address(), registers::isochronous::sync::enable, { |
| 137 | + let mut enables = registers::IsochronousEnables::default(); |
| 138 | + enables.set_operation(true); |
| 139 | + enables.set_sync0(true); |
| 140 | + enables |
| 141 | + }).await.one().unwrap(); |
| 142 | + |
| 143 | +// let period = 2_000_000; |
| 144 | +// let start = raw.read(slave.address(), registers::dc::system_time).await.one().unwrap() as u32; |
| 145 | +// let start = (start / period) * period + period; |
| 146 | +// |
| 147 | +// raw.write(slave.address(), registers::isochronous::all, { |
| 148 | +// let mut isochronous = registers::Isochronous::default(); |
| 149 | +// isochronous.enable.set_operation(true); |
| 150 | +// isochronous.enable.set_sync0(true); |
| 151 | +// isochronous.sync0_cycle_time = period; |
| 152 | +// isochronous.start_time = start; |
| 153 | +// isochronous |
| 154 | +// }).await.one().unwrap(); |
| 155 | + } |
| 156 | + |
| 157 | + println!("switching safeop"); |
| 158 | + master.switch(CommunicationState::SafeOperational).await.unwrap(); |
| 159 | + println!("safeop"); |
| 160 | + |
| 161 | + master.switch(CommunicationState::Operational).await.unwrap(); |
| 162 | + println!("op"); |
| 163 | + |
| 164 | +// for slave in &slaves { |
| 165 | +// let raw = unsafe {slave.raw_master()}; |
| 166 | +// dbg!(raw.read(slave.address(), registers::external_event_mask).await.one()); |
| 167 | +// dbg!(raw.read(slave.address(), registers::isochronous::all).await.one()); |
| 168 | +// } |
| 169 | + |
| 170 | + println!("custom init"); |
| 171 | + for slave in &slaves { |
| 172 | + raw.write(slave.address(), registers::external_event_mask, { |
| 173 | + let mut mask = registers::ExternalEvent::default(); |
| 174 | + mask.set_dc0(true); |
| 175 | + mask.set_dl(true); |
| 176 | + mask.set_sync_manager_channel_at(2, true); |
| 177 | + mask.set_sync_manager_channel_at(3, true); |
| 178 | + mask |
| 179 | + }).await.one()?; |
| 180 | + } |
| 181 | + |
| 182 | + println!("all configured"); |
| 183 | + let clock = master.clock().await; |
| 184 | + let mut interval = tokio_timerfd::Interval::new_interval(Duration::from_millis(2)).unwrap(); |
| 185 | + |
| 186 | + // survey divergence |
| 187 | + loop { |
| 188 | + interval.next().await.unwrap().unwrap(); |
| 189 | + // dynamic drift |
| 190 | + clock.sync().await; |
| 191 | + |
| 192 | + // survey timestamps |
| 193 | + let mut group = group.data().await; |
| 194 | + group.read().await; |
| 195 | + for &time in &offsets { |
| 196 | + print!("{} ", group.get(time)); |
| 197 | + } |
| 198 | + print!("\n"); |
| 199 | + } |
| 200 | + |
| 201 | + Ok(()) |
| 202 | +} |
| 203 | + |
0 commit comments