Feb 1st development in room

src/constants.rs

@@ -65,8 +65,33 @@ pub mod drivetrain {
     pub const PODIUM_SHOT_ANGLE: f64 = 34.34; // degrees
 }
 pub mod elevator {
-    pub const BOTTOM: f64 = 0.0;
-    pub const L2: f64 = 0.0;
-    pub const L3: f64 = 0.0;
-    pub const L4: f64 = 39.7;
+    pub const BOTTOM: f64 = 0.0; // unit is rotations
+    pub const L2: f64 = 1.; // unit is rotations
+    pub const L3: f64 = 15.75; // unit is rotations
+    pub const L4: f64 = 39.7; // unit is rotations
+    pub const ELEVATOR_TRAPEZOID_DT_MS: u64 = 50; // Currently unused TODO: update this comment when elevator trapezoidal move is async
+    pub const POSITION_TOLERANCE: f64 = 0.25; // Currently unused TODO: update this comment when elevator trapezoidal move is async
+}
+pub mod joystick_map {
+    // Joystick IDs (set in driver station)
+    pub const RIGHT_DRIVE: i32 = 0;
+    pub const LEFT_DRIVE: i32 = 1;
+    pub const OPERATOR: i32 = 2;
+
+    //Right drive
+    pub const LINEUP_LEFT: usize = 3;
+    pub const LINEUP_RIGHT: usize = 4;
+
+    //Left drive
+    pub const INDEXER_IN: usize = 1;
+    pub const INDEXER_OUT: usize = 2;
+
+    //Operator
+    pub const ELEVATOR_TRAPEZOID_TO_STORED_TARGET: usize = 1;
+    pub const ELEVATOR_UP_MANUAL: usize = 3;
+    pub const ELEVATOR_DOWN_MANUAL: usize = 4;
+    pub const CLIMB_FULL: usize = 8;
+    pub const SET_TARGET_L2: usize = 14;
+    pub const SET_TARGET_L3: usize = 15;
+    pub const SET_TARGET_L4: usize = 16;
 }

src/lib.rs

@@ -4,17 +4,17 @@ pub mod container;
 pub mod subsystems;
 pub mod swerve;
 
-use std::cell::RefCell;
-
 use crate::auto::Auto;
 use crate::container::control_drivetrain;
 use crate::subsystems::{
-    Climber, Drivetrain, DrivetrainControlState, Elevator, Indexer, LineupSide,
+    Climber, Drivetrain, DrivetrainControlState, Elevator, ElevatorPosition, Indexer, LineupSide,
 };
 use frcrs::input::Joystick;
 use frcrs::networktables::NetworkTable;
 use frcrs::telemetry::Telemetry;
 use frcrs::{Robot, TaskManager};
+use std::cell::RefCell;
+use std::cmp::PartialEq;
 use std::ops::Deref;
 use std::rc::Rc;
 use std::sync::Arc;
@@ -58,9 +58,9 @@ impl Ferris {
         Ferris {
             task_manager: TaskManager::new(),
             controllers: Controllers {
-                left_drive: Joystick::new(1),
-                right_drive: Joystick::new(0),
-                operator: Joystick::new(2),
+                left_drive: Joystick::new(constants::joystick_map::LEFT_DRIVE),
+                right_drive: Joystick::new(constants::joystick_map::RIGHT_DRIVE),
+                operator: Joystick::new(constants::joystick_map::OPERATOR),
             },
             drivetrain: Rc::new(RefCell::new(Drivetrain::new())),
             elevator: Rc::new(RefCell::new(Elevator::new())),
@@ -156,30 +156,87 @@ impl Robot for Ferris {
             //drivetrain.update_limelight().await;
             drivetrain.post_odo().await;
 
-            if self.controllers.right_drive.get(3) {
+            if self
+                .controllers
+                .right_drive
+                .get(constants::joystick_map::LINEUP_LEFT)
+            {
                 drivetrain.lineup(LineupSide::Left).await;
-            } else if self.controllers.right_drive.get(4) {
+            } else if self
+                .controllers
+                .right_drive
+                .get(constants::joystick_map::LINEUP_RIGHT)
+            {
                 drivetrain.lineup(LineupSide::Right).await;
             } else {
                 control_drivetrain(&mut drivetrain, &mut self.controllers, drivetrain_state).await;
             }
         }
 
-        if let Ok(elevator) = self.elevator.try_borrow_mut() {
-            if self.controllers.operator.get(3) {
-                elevator.set_speed(0.5);
-            } else if self.controllers.operator.get(4) {
-                elevator.set_speed(-0.5);
+        if let Ok(mut elevator) = self.elevator.try_borrow_mut() {
+            // Setting the target position
+            if self
+                .controllers
+                .operator
+                .get(constants::joystick_map::SET_TARGET_L2)
+            {
+                elevator.set_target(ElevatorPosition::L2);
+            } else if self
+                .controllers
+                .operator
+                .get(constants::joystick_map::SET_TARGET_L3)
+            {
+                elevator.set_target(ElevatorPosition::L3);
+            } else if self
+                .controllers
+                .operator
+                .get(constants::joystick_map::SET_TARGET_L4)
+            {
+                elevator.set_target(ElevatorPosition::L4);
+            }
+
+            // Setting the actual elevator operation
+            if self
+                .controllers
+                .operator
+                .get(constants::joystick_map::ELEVATOR_UP_MANUAL)
+            {
+                // Manual up
+                elevator.set_speed(0.1);
+            } else if self
+                .controllers
+                .operator
+                .get(constants::joystick_map::ELEVATOR_DOWN_MANUAL)
+            {
+                // Manual down
+                elevator.set_speed(-0.1);
+            } else if self
+                .controllers
+                .operator
+                .get(constants::joystick_map::ELEVATOR_TRAPEZOID_TO_STORED_TARGET)
+            {
+                // Trapezoidal to stored target
+                elevator.run_to_target_trapezoid();
             } else {
                 elevator.set_speed(0.0);
             }
         }
 
         if let Ok(indexer) = self.indexer.try_borrow_mut() {
-            if self.controllers.operator.get(2) {
+            if self
+                .controllers
+                .left_drive
+                .get(constants::joystick_map::INDEXER_OUT)
+            {
+                // Out the front
                 indexer.set_speed(0.3);
-            } else if self.controllers.operator.get(1) {
-                indexer.set_speed(-0.1);
+            } else if self
+                .controllers
+                .left_drive
+                .get(constants::joystick_map::INDEXER_IN)
+            {
+                // In, score out the left
+                indexer.set_speed(-0.5);
             } else {
                 indexer.set_speed(0.0);
             }
@@ -214,7 +271,11 @@ impl Robot for Ferris {
             }
         };
 
-        if self.controllers.operator.get(8) {
+        if self
+            .controllers
+            .operator
+            .get(constants::joystick_map::CLIMB_FULL)
+        {
             self.task_manager.run_task(climb);
         } else {
             self.task_manager.run_task(cancel_climb);

src/subsystems/drivetrain.rs

@@ -224,12 +224,14 @@ impl Drivetrain {
     }
 
     pub fn set_speeds(&mut self, fwd: f64, str: f64, rot: f64) {
-        //println!("ODO X: {}", self.odometry.position.x);
+        println!(
+            "ODO XY: {}, {}",
+            self.odometry.position.x, self.odometry.position.y
+        );
         let mut transform = Vector2::new(-str, fwd);
-        transform =
-            Rotation2::new(-self.get_offset().get::<radian>() + std::f64::consts::PI) * transform;
+        transform = Rotation2::new(self.get_offset().get::<radian>()) * transform;
 
-        let wheel_speeds = self.kinematics.calculate(transform, rot);
+        let wheel_speeds = self.kinematics.calculate(transform, -rot);
 
         let measured = self.get_speeds();
 
@@ -301,9 +303,9 @@ impl Drivetrain {
 
     pub fn get_angle(&self) -> Angle {
         if alliance_station().red() {
-            Angle::new::<degree>(-self.pigeon.get_rotation().z + 180.)
+            Angle::new::<radian>(-self.pigeon.get_rotation().z + 180.)
         } else {
-            Angle::new::<degree>(-self.pigeon.get_rotation().z)
+            Angle::new::<radian>(-self.pigeon.get_rotation().z)
         }
     }
 

src/subsystems/elevator.rs

@@ -1,11 +1,16 @@
 use crate::constants::{elevator, robotmap};
 use frcrs::ctre::{ControlMode, Talon};
+use std::fmt::Display;
+use std::time::Duration;
+use tokio::time::sleep;
 
 pub struct Elevator {
     left: Talon,
     right: Talon,
+    target: ElevatorPosition,
 }
 
+#[derive(Copy, Clone, Debug, PartialEq)]
 pub enum ElevatorPosition {
     Bottom,
     L2,
@@ -22,24 +27,58 @@ impl Default for Elevator {
 impl Elevator {
     pub fn new() -> Self {
         let left = Talon::new(robotmap::elevator::LEFT, Some("can0".to_string()));
-        let right = Talon::new(robotmap::elevator::RIGHT, Some("can0".to_string()));
+        let right = Talon::new(robotmap::elevator::RIGHT, Some("can0".to_string())); //should be inverted (clockwise positive) in config
 
-        right.follow(&left, true);
+        //right.follow(&left, true);
 
-        Self { left, right }
+        Self {
+            left,
+            right,
+            target: ElevatorPosition::Bottom,
+        }
+    }
+    /// Set the stored goal position.
+    /// TODO: update the below lines of this comment once controls scheme is figured out.
+    /// That position is referenced by run_to_target_trapezoid.
+    /// Not much benefit over just passing a target to that function yet but I have plans.
+    pub fn set_target(&mut self, target: ElevatorPosition) {
+        self.target = target;
+    }
+    /// Get the stored goal position.
+    /// For stuff like LEDs.
+    /// Indexer used to reference this to get scoring speed, might need to go back to that once we have real bumpers
+    pub fn get_target(&self) -> ElevatorPosition {
+        self.target
     }
 
-    pub fn set_position(&self, position: ElevatorPosition) {
-        match position {
-            ElevatorPosition::Bottom => self.left.set(ControlMode::Position, elevator::BOTTOM),
-            ElevatorPosition::L2 => self.left.set(ControlMode::Position, elevator::L2),
-            ElevatorPosition::L3 => self.left.set(ControlMode::Position, elevator::L3),
-            ElevatorPosition::L4 => self.left.set(ControlMode::Position, elevator::L4),
+    /// Runs a trapezoidal (motion magic) profile on the elevator krakens to move the elevator to its stored target position.
+    /// Most of the interesting stuff for this is in the elevator krakens' configurations (set in pheonix tuner).
+    /// Those configuration files have been saved to Documents on the driver station laptop.
+    pub fn run_to_target_trapezoid(&mut self) {
+        //load position to run to in rotations from constants.rs
+        let target_position = match self.get_target() {
+            ElevatorPosition::Bottom => elevator::BOTTOM,
+            ElevatorPosition::L2 => elevator::L2,
+            ElevatorPosition::L3 => elevator::L3,
+            ElevatorPosition::L4 => elevator::L4,
+        };
+
+        // current implementation is to just set the control mode and call this function every frame
+        self.right.set(ControlMode::MotionMagic, target_position);
+        self.left.set(ControlMode::MotionMagic, target_position);
+
+        // below is for when we eventually make this function async
+        /*
+        while (self.right.get_position() - target_position).abs() < elevator::POSITION_TOLERANCE {
+            self.right.set(ControlMode::MotionMagic, target_position);
+            self.left.set(ControlMode::MotionMagic, target_position);
         }
+         */
     }
 
     pub fn set_speed(&self, speed: f64) {
         self.left.set(ControlMode::Percent, speed);
+        self.right.set(ControlMode::Percent, speed); //is inverted in config
     }
 
     pub fn stop(&self) {