kernelvex 0.3.0

VEX Controls library
Documentation 
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

extern crate kernelvex as kv;
use kv::{
    DifferentialDrive, MotorGroup, OdomChassis, Pose, QAngle, QLength,
    QTime, Tank, Trajectory, TrajectoryPoint,
};
use kv::ExpoDrive;
use vexide::prelude::*;
use kv::OmniWheel::Omni325;

struct Robot {
    controller: Controller,
    dt: DifferentialDrive,
    chassis: OdomChassis,
}

impl Compete for Robot {
    async fn autonomous(&mut self) {
        // Drive straight 2 meters using trapezoidal profile
        let _ = self
            .chassis
            .shoot(QLength::from_meters(2.))
            .await;

        // Turn to 90 degrees
        let _ = self
            .chassis
            .turn(QAngle::from_degrees(90.))
            .await;

        let traj = Trajectory::from_points(vec![
            TrajectoryPoint::new(
                Pose::new(Default::default(), QAngle::from_degrees(0.0)),
                0.0,
                0.0,
                QTime::from_sec(0.0),
            ),
            TrajectoryPoint::new(
                Pose::new(Default::default(), QAngle::from_degrees(0.0)),
                0.6,
                0.0,
                QTime::from_sec(2.0),
            ),
        ]);

        // Follow path
        let _ = self
            .chassis
            .trajectory(&traj)
            .await;
    }

    async fn driver(&mut self) {
        loop {
            let state = self.controller.state().unwrap_or_default();
            _ = self
                .dt
                .drive_tank(state.left_stick.y(), state.right_stick.x());

            sleep(Controller::UPDATE_INTERVAL).await;
        }
    }
}

#[vexide::main(banner(enabled = true))]
async fn main(peripherals: Peripherals) {
    let l_motor = MotorGroup::new([
        Motor::new(peripherals.port_11, Gearset::Blue, Direction::Reverse),
        Motor::new(peripherals.port_6, Gearset::Blue, Direction::Reverse),
        Motor::new(peripherals.port_12, Gearset::Blue, Direction::Reverse),
    ]);
    let r_motor = MotorGroup::new([
        Motor::new(peripherals.port_19, Gearset::Blue, Direction::Reverse),
        Motor::new(peripherals.port_10, Gearset::Blue, Direction::Reverse),
        Motor::new(peripherals.port_9, Gearset::Blue, Direction::Reverse),
    ]);

    let mut imu = InertialSensor::new(peripherals.port_1);
    let _ = imu.calibrate().await;

    let chassis = OdomChassis::with_config(
        DifferentialDrive::new(l_motor.clone(),
                               r_motor.clone(),
                               ExpoDrive::new(1.5, 1.7, None),
                               Omni325,
                               QLength::from_meters(0.35),
                               1.),
        imu,
        None,
    );

    let robot = Robot {
        controller: peripherals.primary_controller,
        dt: DifferentialDrive::new(l_motor.clone(),
                                   r_motor.clone(),
                                   ExpoDrive::new(1.5, 1.7, None),
                                   Omni325,
                                   QLength::from_meters(0.35),
                                   1.),
        chassis,
    };

    robot.compete().await;
}