SHARE
TWEET

Untitled

a guest May 21st, 2019 94 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #[derive(Debug, Copy, Clone)]
  2. pub enum RpiV1P1 {  // RPi Version 1, plug P1
  3.     P1_03,  // Pin P1-03
  4.     P1_05,  // Pin P1-05
  5.     P1_07,  // Pin P1-07
  6.     P1_08,  // Pin P1-08, defaults to alt function 0 UART0_TXD
  7.     P1_10,  // Pin P1-10, defaults to alt function 0 UART0_RXD
  8.     P1_11,  // Pin P1-11
  9.     P1_12,  // Pin P1-12, can be PWM channel 0 in ALT FUN 5
  10.     P1_13,  // Pin P1-13
  11.     P1_15,  // Pin P1-15
  12.     P1_16,  // Pin P1-16
  13.     P1_18,  // Pin P1-18
  14.     P1_19,  // Pin P1-19, MOSI when SPI0 in use
  15.     P1_21,  // Pin P1-21, MISO when SPI0 in use
  16.     P1_22,  // Pin P1-22
  17.     P1_23,  // Pin P1-23, CLK when SPI0 in use
  18.     P1_24,  // Pin P1-24, CE0 when SPI0 in use
  19.     P1_26,  // Pin P1-26, CE1 when SPI0 in use
  20. }
  21.  
  22. #[derive(Debug, Copy, Clone)]
  23. pub enum RpiV2P1 {  // RPi Version 2, plug P1
  24.     P1_03,  // Pin P1-03
  25.     P1_05,  // Pin P1-05
  26.     P1_07,  // Pin P1-07
  27.     P1_08,  // Pin P1-08, defaults to alt function 0 UART0_TXD
  28.     P1_10,  // Pin P1-10, defaults to alt function 0 UART0_RXD
  29.     P1_11,  // Pin P1-11
  30.     P1_12,  // Pin P1-12, can be PWM channel 0 in ALT FUN 5
  31.     P1_13,  // Pin P1-13
  32.     P1_15,  // Pin P1-15
  33.     P1_16,  // Pin P1-16
  34.     P1_18,  // Pin P1-18
  35.     P1_19,  // Pin P1-19, MOSI when SPI0 in use
  36.     P1_21,  // Pin P1-21, MISO when SPI0 in use
  37.     P1_22,  // Pin P1-22
  38.     P1_23,  // Pin P1-23, CLK when SPI0 in use
  39.     P1_24,  // Pin P1-24, CE0 when SPI0 in use
  40.     P1_26,  // Pin P1-26, CE1 when SPI0 in use
  41.     P1_29,  // Pin P1-29
  42.     P1_31,  // Pin P1-31
  43.     P1_32,  // Pin P1-32
  44.     P1_33,  // Pin P1-33
  45.     P1_35,  // Pin P1-35, can be PWM channel 1 in ALT FUN 5
  46.     P1_36,  // Pin P1-36
  47.     P1_37,  // Pin P1-37
  48.     P1_38,  // Pin P1-38
  49.     P1_40,  // Pin P1-40
  50.     // RPi Version 2, new plug P5
  51.     P5_03,  // Pin P5-03
  52.     P5_04,  // Pin P5-04
  53.     P5_05,  // Pin P5-05
  54.     P5_06,  // Pin P5-06
  55. }
  56.  
  57. #[derive(Debug, Copy, Clone)]
  58. pub enum RpiBPlusJ8 {  // RPi Version 2, plug P1
  59.     J8_03,  // Pin J8-03
  60.     J8_05,  // Pin J8-05
  61.     J8_07,  // Pin J8-07
  62.     J8_08,  // Pin J8-08, defaults to alt function 0 UART0_TXD
  63.     J8_10,  // Pin J8-10, defaults to alt function 0 UART0_RXD
  64.     J8_11,  // Pin J8-11
  65.     J8_12,  // Pin J8-12, can be PWM channel 0 in ALT FUN 5
  66.     J8_13,  // Pin J8-13
  67.     J8_15,  // Pin J8-15
  68.     J8_16,  // Pin J8-16
  69.     J8_18,  // Pin J8-18
  70.     J8_19,  // Pin J8-19, MOSI when SPI0 in use
  71.     J8_21,  // Pin J8-21, MISO when SPI0 in use
  72.     J8_22,  // Pin J8-22
  73.     J8_23,  // Pin J8-23, CLK when SPI0 in use
  74.     J8_24,  // Pin J8-24, CE0 when SPI0 in use
  75.     J8_26,  // Pin J8-26, CE1 when SPI0 in use
  76.     J8_29,  // Pin J8-29
  77.     J8_31,  // Pin J8-31
  78.     J8_32,  // Pin J8-32
  79.     J8_33,  // Pin J8-33
  80.     J8_35,  // Pin J8-35, can be PWM channel 1 in ALT FUN 5
  81.     J8_36,  // Pin J8-36
  82.     J8_37,  // Pin J8-37
  83.     J8_38,  // Pin J8-38
  84.     J8_40,  // Pin J8-40
  85. }
  86.  
  87. use std::marker::PhantomData;
  88.  
  89. pub trait RpiVersion where Self::Pin:Copy+'static {
  90.     type Pin;
  91.     const PINS: &'static[Self::Pin];
  92. }
  93.  
  94. pub struct RpiV1 {}
  95.  
  96. pub struct RpiV2 {}
  97.  
  98. pub struct RpiBPlus {}
  99.  
  100. use std::sync::{Arc, Mutex};
  101.  
  102. fn check_mode(mode: u8) -> Result<(), String> {
  103.     match mode {
  104.         0...7 => Ok(()),
  105.         x => Err(format!("Invalid mode value (must be in range 0..7): {}", x))
  106.     }
  107. }
  108.  
  109. fn check_on(on: u8) -> Result<(), String> {
  110.     match on {
  111.         0...1 => Ok(()),
  112.         x => Err(format!("Invalid on value (must be 1 (high) or 0 (low)): {}", x))
  113.     }
  114. }
  115.  
  116. lazy_static! {
  117.     static ref INITIALIZED: Arc<Mutex<u32>> = Arc::new(Mutex::new(0));
  118. }
  119.  
  120. pub struct Bcm2835 {}
  121.  
  122. impl<V> Gpio<V> for Bcm2835 where V:RpiVersion, V::Pin:private::Supported, u8: std::convert::From<<V as RpiVersion>::Pin> {
  123.  
  124.     fn set_debug(debug: bool)  -> Result<(), String> {
  125.         match *INITIALIZED.lock().unwrap() {
  126.             0 => unsafe {
  127.                 raw::bcm2835_set_debug(debug as u8);
  128.                 Ok(())
  129.             },
  130.             _ => Err("Debug mode can only be set before BCM2835 library initialization".to_string())
  131.         }
  132.     }
  133.  
  134.     fn init() -> Result<Self, String> {
  135.         let mut initialized = INITIALIZED.lock().unwrap();
  136.         match *initialized {
  137.             0 => unsafe {
  138.                 match raw::bcm2835_init() {
  139.                     1 => {
  140.                         *initialized += 1;
  141.                         Ok(Bcm2835{})
  142.                     },
  143.                     x => Err(format!("Initializing BCM2835 library failed: {}", x))
  144.                 }
  145.             },
  146.             _ => {
  147.                 *initialized += 1;
  148.                 Ok(Bcm2835{})
  149.             }
  150.         }
  151.     }
  152.  
  153.     fn fsel(&self, pin: V::Pin, mode: u8) -> Result<(), String> {
  154.         check_mode(mode)?;
  155.         unsafe {
  156.             raw::bcm2835_gpio_fsel(u8::from(pin), mode);
  157.             Ok(())
  158.         }
  159.     }
  160.  
  161.     fn write(&self, pin: V::Pin, on: u8) -> Result<(), String> {
  162.         check_on(on)?;
  163.         unsafe {
  164.             raw::bcm2835_gpio_write(u8::from(pin), on);
  165.             Ok(())
  166.         }
  167.     }
  168.  
  169.     fn delay(&self, millis: u32) {
  170.         unsafe {
  171.             raw::bcm2835_delay(millis);
  172.         }
  173.     }
  174.  
  175.     fn delay_microseconds(&self, micros: u64) {
  176.         unsafe {
  177.             raw::bcm2835_delayMicroseconds(micros);
  178.         }
  179.     }
  180. }
  181.  
  182. impl Drop for Bcm2835 {
  183.     fn drop(&mut self) {
  184.         let mut initialized = INITIALIZED.lock().unwrap();
  185.         *initialized -= 1;
  186.         match *initialized {
  187.             0 => unsafe {
  188.                 match raw::bcm2835_close() {
  189.                     1 => (),
  190.                     x => eprintln!("Closing BCM2835 library failed: {}", x),
  191.                 }
  192.             },
  193.             _ => ()
  194.         }
  195.     }
  196. }
  197.  
  198. mod raw {
  199. #[cfg(not(all(target_arch = "arm", target_vendor = "unknown", target_os = "linux", target_env = "gnu")))]
  200. pub unsafe fn bcm2835_init() -> i32 {
  201.     debug!("bcm2835_init() -> {}", 1);
  202.     1
  203. }
  204.  
  205. #[cfg(not(all(target_arch = "arm", target_vendor = "unknown", target_os = "linux", target_env = "gnu")))]
  206. pub unsafe fn bcm2835_close() -> i32 {
  207.     debug!("bcm2835_close() -> {}", 1);
  208.     1
  209. }
  210.  
  211. #[cfg(not(all(target_arch = "arm", target_vendor = "unknown", target_os = "linux", target_env = "gnu")))]
  212. pub unsafe fn bcm2835_set_debug(debug: u8) {
  213.     debug!("bcm2835_set_debug(debug: {})", debug);
  214. }
  215.  
  216. #[cfg(not(all(target_arch = "arm", target_vendor = "unknown", target_os = "linux", target_env = "gnu")))]
  217. pub unsafe fn bcm2835_gpio_fsel(pin: u8, mode: u8) {
  218.     debug!("bcm2835_gpio_fsel(pin: {}, mode: {})", pin, mode);
  219. }
  220.  
  221. #[cfg(not(all(target_arch = "arm", target_vendor = "unknown", target_os = "linux", target_env = "gnu")))]
  222. pub unsafe fn bcm2835_gpio_write(pin: u8, on: u8) {
  223.     debug!("bcm2835_gpio_write(pin: {}, on: {})", pin, on);
  224. }
  225.  
  226. #[cfg(not(all(target_arch = "arm", target_vendor = "unknown", target_os = "linux", target_env = "gnu")))]
  227. pub unsafe fn bcm2835_delay(millis: u32) {
  228.     debug!("bcm2835_delay(millis: {})", millis);
  229. }
  230.  
  231. #[cfg(not(all(target_arch = "arm", target_vendor = "unknown", target_os = "linux", target_env = "gnu")))]
  232. #[allow(non_snake_case)]
  233. pub unsafe fn bcm2835_delayMicroseconds(micros: u64) {
  234.     debug!("bcm2835_delayMicroseconds(micros: {})", micros);
  235. }
  236.  
  237. }
  238.  
  239. mod private {
  240. macro_rules! enum_values {
  241.     (pub enum $EnumName:ident { $($Variant:ident = $Dicriminant:expr),* $(,)? }) =>
  242.     {
  243.         #[derive(Debug, Copy, Clone)]
  244.         pub enum $EnumName {
  245.             $($Variant = $Dicriminant),*,
  246.         }
  247.         impl $EnumName {
  248.             pub const VALUES: &'static [Self] = &[$($EnumName::$Variant),*];
  249.         }
  250.     }
  251. }
  252.  
  253. enum_values!(
  254.     pub enum RpiV1GpioP1 {  // RPi Version 1
  255.         P1_03 =  0,    // Version 1, Pin P1-03
  256.         P1_05 =  1,    // Version 1, Pin P1-05
  257.         P1_07 =  4,    // Version 1, Pin P1-07
  258.         P1_08 = 14,    // Version 1, Pin P1-08, defaults to alt function 0 UART0_TXD
  259.         P1_10 = 15,    // Version 1, Pin P1-10, defaults to alt function 0 UART0_RXD
  260.         P1_11 = 17,    // Version 1, Pin P1-11
  261.         P1_12 = 18,    // Version 1, Pin P1-12, can be PWM channel 0 in ALT FUN 5
  262.         P1_13 = 21,    // Version 1, Pin P1-13
  263.         P1_15 = 22,    // Version 1, Pin P1-15
  264.         P1_16 = 23,    // Version 1, Pin P1-16
  265.         P1_18 = 24,    // Version 1, Pin P1-18
  266.         P1_19 = 10,    // Version 1, Pin P1-19, MOSI when SPI0 in use
  267.         P1_21 =  9,    // Version 1, Pin P1-21, MISO when SPI0 in use
  268.         P1_22 = 25,    // Version 1, Pin P1-22
  269.         P1_23 = 11,    // Version 1, Pin P1-23, CLK when SPI0 in use
  270.         P1_24 =  8,    // Version 1, Pin P1-24, CE0 when SPI0 in use
  271.         P1_26 =  7     // Version 1, Pin P1-26, CE1 when SPI0 in use
  272.     }
  273. );
  274.  
  275. enum_values!(
  276.     pub enum RpiV2GpioP1 {  // RPi Version 2, new plug P5
  277.         P1_03   =  2,   // Version 2, Pin P1-03
  278.         P1_05   =  3,   // Version 2, Pin P1-05
  279.         P1_07   =  4,   // Version 2, Pin P1-07
  280.         P1_08   = 14,   // Version 2, Pin P1-08, defaults to alt function 0 UART0_TXD
  281.         P1_10   = 15,   // Version 2, Pin P1-10, defaults to alt function 0 UART0_RXD
  282.         P1_11   = 17,   // Version 2, Pin P1-11
  283.         P1_12   = 18,   // Version 2, Pin P1-12, can be PWM channel 0 in ALT FUN 5
  284.         P1_13   = 27,   // Version 2, Pin P1-13
  285.         P1_15   = 22,   // Version 2, Pin P1-15
  286.         P1_16   = 23,   // Version 2, Pin P1-16
  287.         P1_18   = 24,   // Version 2, Pin P1-18
  288.         P1_19   = 10,   // Version 2, Pin P1-19, MOSI when SPI0 in use
  289.         P1_21   =  9,   // Version 2, Pin P1-21, MISO when SPI0 in use
  290.         P1_22   = 25,   // Version 2, Pin P1-22
  291.         P1_23   = 11,   // Version 2, Pin P1-23, CLK when SPI0 in use
  292.         P1_24   =  8,   // Version 2, Pin P1-24, CE0 when SPI0 in use
  293.         P1_26   =  7,   // Version 2, Pin P1-26, CE1 when SPI0 in use
  294.         P1_29   =  5,   // Version 2, Pin P1-29
  295.         P1_31   =  6,   // Version 2, Pin P1-31
  296.         P1_32   = 12,   // Version 2, Pin P1-32
  297.         P1_33   = 13,   // Version 2, Pin P1-33
  298.         P1_35   = 19,   // Version 2, Pin P1-35, can be PWM channel 1 in ALT FUN 5
  299.         P1_36   = 16,   // Version 2, Pin P1-36
  300.         P1_37   = 26,   // Version 2, Pin P1-37
  301.         P1_38   = 20,   // Version 2, Pin P1-38
  302.         P1_40   = 21,   // Version 2, Pin P1-40
  303.         // RPi Version 2, new plug P5
  304.         P5_03   = 28,   // Version 2, Pin P5-03
  305.         P5_04   = 29,   // Version 2, Pin P5-04
  306.         P5_05   = 30,   // Version 2, Pin P5-05
  307.         P5_06   = 31    // Version 2, Pin P5-06
  308.     }
  309. );
  310.  
  311. enum_values!(
  312.     pub enum RpiBPlusGpioJ8 {   // RPi B+ J8 header, also RPi 2 40 pin GPIO header
  313.         J8_03   =  2,   // B+, Pin J8-03
  314.         J8_05   =  3,   // B+, Pin J8-05
  315.         J8_07   =  4,   // B+, Pin J8-07
  316.         J8_08   = 14,   // B+, Pin J8-08, defaults to alt function 0 UART0_TXD
  317.         J8_10   = 15,   // B+, Pin J8-10, defaults to alt function 0 UART0_RXD
  318.         J8_11   = 17,   // B+, Pin J8-11
  319.         J8_12   = 18,   // B+, Pin J8-12, can be PWM channel 0 in ALT FUN 5
  320.         J8_13   = 27,   // B+, Pin J8-13
  321.         J8_15   = 22,   // B+, Pin J8-15
  322.         J8_16   = 23,   // B+, Pin J8-16
  323.         J8_18   = 24,   // B+, Pin J8-18
  324.         J8_19   = 10,   // B+, Pin J8-19, MOSI when SPI0 in use
  325.         J8_21   =  9,   // B+, Pin J8-21, MISO when SPI0 in use
  326.         J8_22   = 25,   // B+, Pin J8-22
  327.         J8_23   = 11,   // B+, Pin J8-23, CLK when SPI0 in use
  328.         J8_24   =  8,   // B+, Pin J8-24, CE0 when SPI0 in use
  329.         J8_26   =  7,   // B+, Pin J8-26, CE1 when SPI0 in use
  330.         J8_29   =  5,   // B+, Pin J8-29,
  331.         J8_31   =  6,   // B+, Pin J8-31,
  332.         J8_32   = 12,   // B+, Pin J8-32,
  333.         J8_33   = 13,   // B+, Pin J8-33,
  334.         J8_35   = 19,   // B+, Pin J8-35, can be PWM channel 1 in ALT FUN 5
  335.         J8_36   = 16,   // B+, Pin J8-36,
  336.         J8_37   = 26,   // B+, Pin J8-37,
  337.         J8_38   = 20,   // B+, Pin J8-38,
  338.         J8_40   = 21    // B+, Pin J8-40,
  339.     }
  340. );
  341.  
  342. pub trait Supported {}
  343.  
  344. impl Supported for RpiV1GpioP1 {}
  345. impl Supported for RpiV2GpioP1 {}
  346. impl Supported for RpiBPlusGpioJ8 {}
  347.  
  348. use crate::{RpiVersion, RpiV1, RpiV2, RpiBPlus};
  349.  
  350. impl RpiVersion for RpiV1 {
  351.     type Pin = RpiV1GpioP1;
  352.     const PINS: &'static[Self::Pin] = RpiV1GpioP1::VALUES;
  353. }
  354.  
  355. impl From<RpiV1GpioP1> for u8 {
  356.     fn from(pin: RpiV1GpioP1) -> Self {
  357.         pin as u8
  358.     }
  359. }
  360.  
  361. impl RpiVersion for RpiV2 {
  362.     type Pin = RpiV2GpioP1;
  363.     const PINS: &'static[Self::Pin] = RpiV2GpioP1::VALUES;
  364. }
  365.  
  366. impl From<RpiV2GpioP1> for u8 {
  367.     fn from(pin: RpiV2GpioP1) -> Self {
  368.         pin as u8
  369.     }
  370. }
  371.  
  372. impl RpiVersion for RpiBPlus {
  373.     type Pin = RpiBPlusGpioJ8;
  374.     const PINS: &'static[Self::Pin] = RpiBPlusGpioJ8::VALUES;
  375. }
  376.  
  377. impl From<RpiBPlusGpioJ8> for u8 {
  378.     fn from(pin: RpiBPlusGpioJ8) -> Self {
  379.         pin as u8
  380.     }
  381. }
  382.  
  383. }
  384.  
  385. pub trait Gpio<V> where V:RpiVersion {
  386.     fn set_debug(debug: bool) -> Result<(), String>;
  387.     fn init() -> Result<Self, String> where Self:Sized;
  388.     fn fsel(&self, pin: V::Pin, mode: u8) -> Result<(), String>;
  389.     fn write(&self, pin: V::Pin, on: u8) -> Result<(), String>;
  390.     fn delay(&self, millis: u32);
  391.     fn delay_microseconds(&self, micros: u64);
  392. }
  393.  
  394. pub trait Driver<'d, 'g, V, G:'g> where V:RpiVersion, G:Gpio<V> {
  395.     type Motor:Motor;
  396.     fn init(gpio: &'g G) -> Result<Self, String> where Self:Sized;
  397.     fn nb_motors(&'d self) -> u8;
  398.     fn get_motor(&'d self, motor: u8) -> Result<Self::Motor, String>;
  399. }
  400.  
  401. pub trait Motor {
  402.     fn init(&self) -> Result<(), String>;
  403.     fn set_micro_step(&self) -> Result<(), String>;
  404.     fn turn_step(&self) -> Result<(), String>;
  405.     fn stop(&self) -> Result<(), String>;
  406. }
  407.  
  408. pub struct Drv8825<'g, V, G:'g> where V:RpiVersion, G: Gpio<V> {
  409.     gpio: &'g G,
  410.     version: PhantomData<V>,
  411. }
  412.  
  413. macro_rules! resolve_gpio_pins {
  414.     ($($ConstRpiPinName:ident => $ConstGpioPinName:ident);* $(;)?) =>
  415.     {
  416.         $(const $ConstGpioPinName:V::Pin = V::PINS[Self::$ConstRpiPinName as usize]);*;
  417.     }
  418. }
  419.  
  420. #[allow(dead_code)]
  421. impl<'g, V, G:'g> Drv8825<'g, V, G> where V:RpiVersion, G: Gpio<V> {
  422.     const NB_MOTORS:u8 = 2;
  423.  
  424.     // RPI pins used for motor 1
  425.     const M1_ENABLE_RPI_PIN:RpiV2P1 = RpiV2P1::P1_32;
  426.     const M1_DIR_RPI_PIN:RpiV2P1    = RpiV2P1::P1_33;
  427.     const M1_STEP_RPI_PIN:RpiV2P1   = RpiV2P1::P1_35;
  428.     const M1_M0_RPI_PIN:RpiV2P1     = RpiV2P1::P1_36;
  429.     const M1_M1_RPI_PIN:RpiV2P1     = RpiV2P1::P1_11;
  430.     const M1_M2_RPI_PIN:RpiV2P1     = RpiV2P1::P1_38;
  431.  
  432.     // RPI pins used for motor 2
  433.     const M2_ENABLE_RPI_PIN:RpiV2P1 = RpiV2P1::P1_07;
  434.     const M2_DIR_RPI_PIN:RpiV2P1    = RpiV2P1::P1_18;
  435.     const M2_STEP_RPI_PIN:RpiV2P1   = RpiV2P1::P1_12;
  436.     const M2_M0_RPI_PIN:RpiV2P1     = RpiV2P1::P1_40;
  437.     const M2_M1_RPI_PIN:RpiV2P1     = RpiV2P1::P1_15;
  438.     const M2_M2_RPI_PIN:RpiV2P1     = RpiV2P1::P1_13;
  439.  
  440.     resolve_gpio_pins!(
  441.         // GPIO pins used for motor 1
  442.         M1_ENABLE_RPI_PIN   => M1_ENABLE_GPIO_PIN;
  443.         M1_DIR_RPI_PIN      => M1_DIR_GPIO_PIN;
  444.         M1_STEP_RPI_PIN     => M1_STEP_GPIO_PIN;
  445.         M1_M0_RPI_PIN       => M1_M0_GPIO_PIN;
  446.         M1_M1_RPI_PIN       => M1_M1_GPIO_PIN;
  447.         M1_M2_RPI_PIN       => M1_M2_GPIO_PIN;
  448.  
  449.         // GPIO pins used for motor 2
  450.         M2_ENABLE_RPI_PIN   => M2_ENABLE_GPIO_PIN;
  451.         M2_DIR_RPI_PIN      => M2_DIR_GPIO_PIN;
  452.         M2_STEP_RPI_PIN     => M2_STEP_GPIO_PIN;
  453.         M2_M0_RPI_PIN       => M2_M0_GPIO_PIN;
  454.         M2_M1_RPI_PIN       => M2_M1_GPIO_PIN;
  455.         M2_M2_RPI_PIN       => M2_M2_GPIO_PIN;
  456.     );
  457.  
  458. }
  459.  
  460. macro_rules! debug_pins {
  461.     ($($ConstRpiPinName:expr, $ConstGpioPinName:expr),* $(,)?) =>
  462.     {
  463.         $(error!("{} = {:?} -> {} = {:?}",
  464.                 String::from(stringify!($ConstRpiPinName)), $ConstRpiPinName,
  465.                 String::from(stringify!($ConstGpioPinName)), u8::from($ConstGpioPinName)));*;
  466.     }
  467. }
  468.  
  469. impl<'d, 'g:'d, V:'static, G:'g> Driver<'d, 'g, V, G> for Drv8825<'g, V, G> where V:RpiVersion, u8: std::convert::From<<V as RpiVersion>::Pin>, G: Gpio<V> {
  470.  
  471.     fn init(gpio: &'g G) -> Result<Drv8825<'g, V, G>, String> {
  472.         debug_pins!(
  473.             // Pins used for motor 1
  474.             Self::M1_ENABLE_RPI_PIN, Self::M1_ENABLE_GPIO_PIN,
  475.             Self::M1_DIR_RPI_PIN, Self::M1_DIR_GPIO_PIN,
  476.             Self::M1_STEP_RPI_PIN, Self::M1_STEP_GPIO_PIN,
  477.             Self::M1_M0_RPI_PIN, Self::M1_M0_GPIO_PIN,
  478.             Self::M1_M1_RPI_PIN, Self::M1_M1_GPIO_PIN,
  479.             Self::M1_M2_RPI_PIN, Self::M1_M2_GPIO_PIN,
  480.             // Pins used for motor 2
  481.             Self::M2_ENABLE_RPI_PIN, Self::M2_ENABLE_GPIO_PIN,
  482.             Self::M2_DIR_RPI_PIN, Self::M2_DIR_GPIO_PIN,
  483.             Self::M2_STEP_RPI_PIN, Self::M2_STEP_GPIO_PIN,
  484.             Self::M2_M0_RPI_PIN, Self::M2_M0_GPIO_PIN,
  485.             Self::M2_M1_RPI_PIN, Self::M2_M1_GPIO_PIN,
  486.             Self::M2_M2_RPI_PIN, Self::M2_M2_GPIO_PIN,
  487.         );
  488.         Ok(Drv8825{gpio: gpio, version: PhantomData})
  489.     }
  490.  
  491.     fn nb_motors(&'d self) -> u8 {
  492.         Self::NB_MOTORS
  493.     }
  494.  
  495.     type Motor = Drv8825Motor<'d, 'g, V, G>;
  496.  
  497.     fn get_motor(&'d self, motor: u8) -> Result<Self::Motor, String> {
  498.         let motor = match motor {
  499.             0 => Ok(Drv8825Motor{driver: self, enable_pin: Self::M1_ENABLE_GPIO_PIN, dir_pin: Self::M1_DIR_GPIO_PIN, step_pin: Self::M1_STEP_GPIO_PIN, m0_pin: Self::M1_M0_GPIO_PIN, m1_pin: Self::M1_M1_GPIO_PIN, m2_pin: Self::M1_M2_GPIO_PIN}),
  500.             1 => Ok(Drv8825Motor{driver: self, enable_pin: Self::M2_ENABLE_GPIO_PIN, dir_pin: Self::M2_DIR_GPIO_PIN, step_pin: Self::M2_STEP_GPIO_PIN, m0_pin: Self::M2_M0_GPIO_PIN, m1_pin: Self::M2_M1_GPIO_PIN, m2_pin: Self::M2_M2_GPIO_PIN}),
  501.             x => Err(format!("Invalid mode value (must be in range 0..1): {}", x))
  502.         }?;
  503.         motor.init()?;
  504.         Ok(motor)
  505.     }
  506. }
  507.  
  508. pub struct Drv8825Motor<'d, 'g, V:'static, G:'g> where V:RpiVersion, G: Gpio<V> {
  509.     driver: &'d Drv8825<'g, V, G>,
  510.     enable_pin: V::Pin,
  511.     dir_pin: V::Pin,
  512.     step_pin: V::Pin,
  513.     m0_pin: V::Pin,
  514.     m1_pin: V::Pin,
  515.     m2_pin: V::Pin,
  516. }
  517.  
  518. impl<'d, 'g, V, G:'g> Motor for Drv8825Motor<'d, 'g, V, G> where V:RpiVersion, G:Gpio<V> {
  519.     fn init(&self) -> Result<(), String> {
  520.         error!("initializing motor");
  521.         self.driver.gpio.fsel(self.enable_pin, 1)?;
  522.         self.driver.gpio.fsel(self.dir_pin, 1)?;
  523.         self.driver.gpio.fsel(self.step_pin, 1)?;
  524.         self.driver.gpio.fsel(self.m0_pin, 1)?;
  525.         self.driver.gpio.fsel(self.m1_pin, 1)?;
  526.         self.driver.gpio.fsel(self.m2_pin, 1)
  527.     }
  528.  
  529.     fn set_micro_step(&self) -> Result<(), String> {
  530.         error!("set_micro_step");
  531.         self.driver.gpio.write(self.m0_pin, 0)?;
  532.         self.driver.gpio.write(self.m1_pin, 0)?;
  533.         self.driver.gpio.write(self.m2_pin, 0)
  534.     }
  535.  
  536.     fn turn_step(&self) -> Result<(), String> {
  537.         error!("turn_step");
  538.         self.driver.gpio.write(self.enable_pin, 0)?;
  539.         self.driver.gpio.write(self.dir_pin, 0)?;
  540.         self.driver.gpio.write(self.step_pin, 1)?;
  541.         self.driver.gpio.delay(5);
  542.         self.driver.gpio.write(self.step_pin, 0)?;
  543.         self.driver.gpio.delay(5);
  544.         Ok(())
  545.     }
  546.  
  547.     fn stop(&self) -> Result<(), String> {
  548.         error!("stopping motor");
  549.         self.driver.gpio.write(self.enable_pin, 1)
  550.     }
  551. }
  552.  
  553. impl<'d, 'g, V, G:'g> Drop for Drv8825Motor<'d, 'g, V, G> where V:RpiVersion, G:Gpio<V> {
  554.     fn drop(&mut self) {
  555.         match Motor::stop(self) {
  556.             Ok(_) => (),
  557.             Err(msg) => eprintln!("Droping Fail motor failed: {}", msg)
  558.         }
  559.     }
  560. }
  561. #[macro_use] extern crate lazy_static;
  562. #[macro_use] extern crate log;
  563. extern crate env_logger;
  564.  
  565. fn main() -> Result<(), String> {
  566.     env_logger::init();
  567.     let gpio = initialize_gpio::<RpiV2, Bcm2835>()?;
  568.     let driver = initialize_driver::<RpiV2, Bcm2835, Drv8825<RpiV2, Bcm2835>>(&gpio)?;
  569.     driver.nb_motors();
  570.     let motor1 = driver.get_motor(0)?;
  571.     motor1.set_micro_step()?;
  572.     motor1.turn_step()?;
  573.     Ok(())
  574. }
  575.  
  576. fn initialize_gpio<V, G>() -> Result<G, String> where V:RpiVersion, G:Gpio<V> {
  577.     G::set_debug(false)?;
  578.     let gpio: G = Gpio::init()?;
  579.     Ok(gpio)
  580. }
  581.  
  582. fn initialize_driver<'d, 'g, V, G:'g, D>(gpio: &'g G) -> Result<D, String> where V:RpiVersion, D:Driver<'d, 'g, V, G>, G:Gpio<V> {
  583.     let driver: D = Driver::init(gpio)?;
  584.     Ok(driver)
  585. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top