foc 0.3.0

An implementation of various Field Oriented Control algorithms.
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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152

//! Park and Clarke transformations (along with their inverses).
//!
//! The algorithms implemented here are based on [Microsemi's suggested implementation](https://www.microsemi.com/document-portal/doc_view/132799-park-inverse-park-and-clarke-inverse-clarke-transformations-mss-software-implementation-user-guide)

use crate::{FRAC_1_SQRT_3, SQRT_3};

use fixed::types::I16F16;

/// A value in a reference frame that moves with the electrical angle of the
/// motor. The two axes are orthogonal.
#[derive(Debug, Clone)]
pub struct RotatingReferenceFrame {
    pub d: I16F16,
    pub q: I16F16,
}

/// A value in a reference frame that is stationary. The two axes are
/// orthogonal.
#[derive(Debug, Clone)]
pub struct TwoPhaseReferenceFrame {
    pub alpha: I16F16,
    pub beta: I16F16,
}

/// A three-phase value in a stationary reference frame. The values do not
/// necessarily sum to 0.
#[derive(Debug, Clone)]
pub struct ThreePhaseReferenceFrame {
    pub a: I16F16,
    pub b: I16F16,
    pub c: I16F16,
}

/// A three-phase value in a stationary reference frame, where the three values
/// sum to 0. As such, the third value is not given.
#[derive(Debug, Clone)]
pub struct ThreePhaseBalancedReferenceFrame {
    pub a: I16F16,
    pub b: I16F16,
}

/// Clarke transform
///
/// Implements equations 1-4 from the Microsemi guide.
pub fn clarke(inputs: ThreePhaseBalancedReferenceFrame) -> TwoPhaseReferenceFrame {
    TwoPhaseReferenceFrame {
        // Eq3
        alpha: inputs.a,
        // Eq4
        beta: FRAC_1_SQRT_3 * (inputs.a + 2 * inputs.b),
    }
}

/// Inverse Clarke transform
///
/// Implements equations 5-7 from the Microsemi guide.
pub fn inverse_clarke(inputs: TwoPhaseReferenceFrame) -> ThreePhaseReferenceFrame {
    ThreePhaseReferenceFrame {
        // Eq5
        a: inputs.alpha,
        // Eq6
        b: (-inputs.alpha + SQRT_3 * inputs.beta) / 2,
        // Eq7
        c: (-inputs.alpha - SQRT_3 * inputs.beta) / 2,
    }
}

/// Park transform
///
/// Implements equations 8 and 9 from the Microsemi guide.
pub fn park(
    cos_angle: I16F16,
    sin_angle: I16F16,
    inputs: TwoPhaseReferenceFrame,
) -> RotatingReferenceFrame {
    RotatingReferenceFrame {
        // Eq8
        d: cos_angle * inputs.alpha + sin_angle * inputs.beta,
        // Eq9
        q: cos_angle * inputs.beta - sin_angle * inputs.alpha,
    }
}

/// Inverse Park transform
///
/// Implements equations 10 and 11 from the Microsemi guide.
pub fn inverse_park(
    cos_angle: I16F16,
    sin_angle: I16F16,
    inputs: RotatingReferenceFrame,
) -> TwoPhaseReferenceFrame {
    TwoPhaseReferenceFrame {
        // Eq10
        alpha: cos_angle * inputs.d - sin_angle * inputs.q,
        // Eq11
        beta: sin_angle * inputs.d + cos_angle * inputs.q,
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[track_caller]
    fn clark_e_round_trip(a: f32, b: f32) {
        let input = ThreePhaseBalancedReferenceFrame {
            a: I16F16::from_num(a),
            b: I16F16::from_num(b),
        };
        let two_phase = clarke(input.clone());
        dbg!(&two_phase);
        let result = inverse_clarke(two_phase);

        dbg!(&result);

        assert!(result.a.abs_diff(input.a) < 0.0001);
        assert!(result.b.abs_diff(input.b) < 0.0001);
    }

    #[test]
    fn clarke_round_trip_zero() {
        clark_e_round_trip(0., 0.);
    }

    #[test]
    fn clarke_round_trip_two_inputs() {
        clark_e_round_trip(0., 1.);
        clark_e_round_trip(1., 0.);
        clark_e_round_trip(-0.5, -0.5);
        clark_e_round_trip(-0.1, -0.2);
        clark_e_round_trip(13., 21.);
    }

    #[test]
    fn park_round_trip() {
        let angle = I16F16::from_num(0.82);
        let (sin_angle, cos_angle) = cordic::sin_cos(angle);

        let input = TwoPhaseReferenceFrame {
            alpha: I16F16::from_num(2),
            beta: I16F16::from_num(3),
        };
        let moving_reference = park(cos_angle, sin_angle, input.clone());
        dbg!(&moving_reference);
        let result = inverse_park(cos_angle, sin_angle, moving_reference);

        dbg!(&result);

        assert!(result.alpha.abs_diff(input.alpha) < 0.001);
        assert!(result.beta.abs_diff(input.beta) < 0.001);
    }
}