prinThor 0.0.3

The highly reliable but not necessarily functional 3D Printer firmware
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

//! TODO: This feature is still very experimental/preliminar
use crate::hwa;
use crate::hwa::device::UartTrinamic;
#[cfg(feature = "trinamic-uart-multi-channel")]
use crate::hwa::device::AxisChannel;

pub enum TrinamicError {
    Timeout,
    WriteError,
    ReadError,
}

pub struct TrinamicController
{
    uart: UartTrinamic,
}
impl TrinamicController
{
    pub fn new(uart: UartTrinamic) -> Self {
        Self{uart}
    }

    pub async fn init(&mut self) -> Result<(),TrinamicError> {
        hwa::info!("Trinamic_uart CMD");

        #[cfg(feature = "trinamic-uart-multi-channel")]
        self.uart.set_axis_channel(Some(AxisChannel::TMCUartX));

        let _status = self.read_register::<tmc2209::reg::DRV_STATUS>(0).await?;

        let mut gconf = tmc2209::reg::GCONF::default();
        gconf.set_en_spread_cycle(true);
        gconf.set_shaft(false);
        gconf.set_mstep_reg_select(true);
        let _ = self.write_register(0, gconf).await?;
        let _ = self.write_register(1, gconf).await?;
        let _ = self.write_register(2, gconf).await?;
        let _ = self.write_register(3, gconf).await?;

        let mut chopconf = tmc2209::reg::CHOPCONF::default();
        chopconf.set_intpol(true);
        chopconf.set_mres(16);
        let _ = self.write_register(0, chopconf).await?;
        let _ = self.write_register(1, chopconf).await?;
        let _ = self.write_register(2, chopconf).await?;
        let _ = self.write_register(3, chopconf).await?;
        Ok(())
    }

    async fn write_register<T: tmc2209::WritableRegister>(&mut self, slave_addr: u8, reg: T) -> Result<(), TrinamicError>
    {
        self.raw_write(tmc2209::WriteRequest::new(slave_addr, reg).bytes()).await
    }

    async fn read_register<T: tmc2209::ReadableRegister + core::fmt::Debug>(&mut self, slave_addr: u8) -> Result<T, TrinamicError>
    {
        self.raw_write(tmc2209::read_request::<T>(slave_addr).bytes()).await?;
        hwa::debug!("Now reading...");
        let mut buff:[u8; 8] = [0; 8];
        let mut reader = tmc2209::Reader::default();
        loop {
            match embassy_time::with_timeout(
                embassy_time::Duration::from_secs(30),
                self.uart.read_until_idle(&mut buff)).await {
                Ok(Ok(num_bytes_read)) => {
                    if num_bytes_read > 0 {
                        hwa::debug!("Uart read {} bytes", num_bytes_read);
                        match reader.read_response(&buff[0..num_bytes_read]) {
                            (_n, Some(response)) => {
                                return match response.register::<T>() {
                                    Ok(r) => {
                                        let x = alloc::format!("{:?}", r);
                                        hwa::debug!("response[0]. l={} : {}", _n, x.as_str());
                                        Ok(r)
                                    }
                                    _ => {
                                        Err(TrinamicError::ReadError)
                                    }
                                }
                            }
                            (n, None) => {
                                let x = alloc::format!("{:?}", reader.awaiting());
                                hwa::warn!("Uncompleted. (readed {}: {:?}) awaiting {}", n, &buff[0..num_bytes_read], x.as_str());
                            }
                        }
                    }
                }
                Ok(Err(err)) => {
                    let err = alloc::format!("{:?}", err);
                    hwa::error!("read error reading trinamic: {:?}", err.as_str());
                    return Err(TrinamicError::ReadError)
                }
                Err(_) => {
                    hwa::error!("timeout waiting for trinamic response");
                    return Err(TrinamicError::Timeout)
                }
            }
        }
    }

    async fn raw_write(&mut self, bytes: &[u8]) -> Result<(), TrinamicError> {
        hwa::debug!("Trinamic_uart sent {:?}", bytes);
        self.uart.write(bytes).await.map_err(|_| TrinamicError::WriteError)?;
        Ok(self.uart.blocking_flush().map_err(|_| TrinamicError::WriteError)?)
    }
}