Lasercan

src/constants.rs

@@ -24,6 +24,7 @@ pub mod robotmap {
 
     pub mod indexer {
         pub const MOTOR: i32 = 12;
+        pub const DISTANCE: i32 = 0;
     }
 
     pub mod climber {

src/subsystems/indexer.rs

@@ -1,8 +1,13 @@
 use crate::constants::robotmap;
 use frcrs::ctre::{ControlMode, Talon};
+use frcrs::laser_can::LaserCan;
+use std::time::Duration;
+use std::time::Instant;
+use tokio::time::sleep;
 
 pub struct Indexer {
     motor: Talon,
+    laser_can: LaserCan,
 }
 
 impl Default for Indexer {
@@ -15,13 +20,28 @@ impl Indexer {
     pub fn new() -> Self {
         let motor = Talon::new(robotmap::indexer::MOTOR, None);
 
-        Self { motor }
+        Self { motor, laser_can }
     }
 
     pub fn set_speed(&self, speed: f64) {
         self.motor.set(ControlMode::Percent, speed);
     }
 
+    pub async fn intake_coral(&self) {
+        let mut last_loop = Instant::now();
+
+        while self.laser_can.get_measurement() > robotmap::indexer::DISTANCE {
+            self.motor.set(ControlMode::Percent, 1.0); //may be -1 need to test
+
+            // Cap at 250 hz
+            let elapsed = last_loop.elapsed().as_secs_f64();
+            let left = (1. / 250. - elapsed).max(0.);
+            sleep(Duration::from_secs_f64(left)).await;
+            last_loop = Instant::now();
+        }
+        self.motor.stop();
+    }
+
     pub fn stop(&self) {
         self.motor.stop();
     }