cu-rp-balancebot 0.14.0

This is a full robot example for the Copper project. It runs on the Raspberry Pi with the balance bot hat to balance a rod.
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

use cu_ads7883_new::ADSReadingPayload;
use cu_pid::{GenericPIDTask, PIDControlOutputPayload};
use cu_rp_encoder::EncoderPayload;
use cu_rp_sn754410_new::MotorPayload;
use cu29::prelude::*;
use cu29::units::si::f32::Ratio;
use cu29::units::si::ratio::ratio;

pub type BalPID = GenericPIDTask<ADSReadingPayload>;
pub type PosPID = GenericPIDTask<EncoderPayload>;

pub mod monitor {
    #[cfg(feature = "bevymon")]
    pub type AppMonitor = cu_bevymon::CuBevyMon;

    #[cfg(not(feature = "bevymon"))]
    pub type AppMonitor = cu_consolemon::CuConsoleMon;
}

#[derive(Reflect)]
pub struct PIDMerger {}

impl Freezable for PIDMerger {}

impl CuTask for PIDMerger {
    type Resources<'r> = ();
    type Input<'m> = input_msg!('m, PIDControlOutputPayload, PIDControlOutputPayload);
    type Output<'m> = output_msg!(MotorPayload);

    fn new(_config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
    where
        Self: Sized,
    {
        Ok(Self {})
    }

    fn process(
        &mut self,
        _ctx: &CuContext,
        input: &Self::Input<'_>,
        output: &mut Self::Output<'_>,
    ) -> CuResult<()> {
        let (bal_pid_msg, pos_pid_msg) = *input;
        let bal_pid = match bal_pid_msg.payload() {
            Some(payload) => payload,
            None => return Err(CuError::from("Safety mode [balance].")),
        };
        let pos_pid = match pos_pid_msg.payload() {
            Some(payload) => payload,
            None => return Err(CuError::from("Safety mode [rail].")),
        };
        output.tov = merge_tov(bal_pid_msg.tov, pos_pid_msg.tov);
        let composite_output = (bal_pid.output - pos_pid.output).clamp(-1.0, 1.0);
        output.set_payload(MotorPayload {
            power: Ratio::new::<ratio>(composite_output),
        });
        output
            .metadata
            .set_status(format!("Comp:{composite_output:.2}"));

        Ok(())
    }
}

fn merge_tov(left: Tov, right: Tov) -> Tov {
    match (left, right) {
        (Tov::None, other) | (other, Tov::None) => other,
        (Tov::Time(a), Tov::Time(b)) => {
            let (start, end) = if a <= b { (a, b) } else { (b, a) };
            Tov::Range(CuTimeRange { start, end })
        }
        (Tov::Range(range), Tov::Time(time)) | (Tov::Time(time), Tov::Range(range)) => {
            Tov::Range(CuTimeRange {
                start: range.start.min(time),
                end: range.end.max(time),
            })
        }
        (Tov::Range(left), Tov::Range(right)) => Tov::Range(CuTimeRange {
            start: left.start.min(right.start),
            end: left.end.max(right.end),
        }),
    }
}