foc_simple/foc/
foc_simple.rs

1use fixed::types::I16F16;
2use rtt_target::rprintln;
3
4use crate::{
5  foc::{EDir, EFocMode},
6  tools::foc_pid::FocPid,
7  EFocAngle, EFocSimpleError, Result, ShaftPosition,
8};
9
10pub struct FocSimple {
11  // user request
12  shaft_position_req: ShaftPosition,
13  foc_mode: EFocMode,
14  calibration_state: ECalibrateState,
15  torque: I16F16, // target for torque in %
16  target_pid: FocPid,
17  // angle sensor
18  shaft_position_act: ShaftPosition,
19  velocity: I16F16, // rad / s. Filtered with a lowpass filter of ca 10 Hrz
20  nr_poles: usize,
21  // current sensor
22  // internal state
23  electrical_offset: I16F16, // offset of the angle sensor with respect to the poles of the motor in radians.
24  timestamp_vel: usize,
25  speed_req: I16F16,
26  speed_acc: I16F16, // in rad/100 ms
27  speed_act: I16F16,
28  temp: I16F16,
29}
30
31#[derive(Debug)]
32#[repr(u8)]
33enum ECalibrateState {
34  Init = 0,
35  FindDirection,
36  FindOffset,
37  ReturnToStart,
38}
39
40impl FocSimple {
41  pub fn new(nr_poles: usize) -> FocSimple {
42    FocSimple {
43      // user request parameters
44      shaft_position_req: ShaftPosition::new(),
45      torque: I16F16::ZERO,
46      foc_mode: EFocMode::Idle,
47      electrical_offset: I16F16::ZERO,
48      velocity: I16F16::ZERO, // in rad per second
49      shaft_position_act: ShaftPosition::new(),
50      nr_poles,
51      // current sensor
52      // internal state
53      calibration_state: ECalibrateState::Init,
54      target_pid: FocPid::new(I16F16::ONE, I16F16::ONE, I16F16::ONE),
55      timestamp_vel: 0,
56      temp: I16F16::ZERO,
57      speed_req: I16F16::ZERO,
58      speed_acc: I16F16::ONE / 10, // in rad/100 ms
59      speed_act: I16F16::ZERO,
60    }
61  }
62
63  /// Velocity mode: Set the speed in rad/sec, positive or negative
64  /// Speed wil increment or decrement until this target value is reached
65  pub fn set_speed(&mut self, speed: I16F16) {
66    self.speed_req = speed;
67  }
68  /// Torque mode: set the torque in range -1 ..1
69  /// Calibration mode: Set the torque for calibration
70  /// Torque is set immediatly
71  pub fn set_torque(&mut self, torque: I16F16) {
72    self.torque = torque;
73  }
74  /// Angle mode: set the angle in range 0 .. TAU
75  /// Angle wil be set immediatly. Speed of change can be regulated with torque limit
76  pub fn set_angle(&mut self, angle: I16F16) {
77    if let EFocMode::Angle(_) = self.foc_mode {
78      self.shaft_position_req.angle = angle;
79    }
80  }
81  /// Angle mode: set the position with shaft position. Can be positive or negative
82  /// Angle wil be set immediatly. Speed of change can be regulated with torque limit
83  pub fn set_position(&mut self, position: ShaftPosition) {
84    if let EFocMode::Angle(_) = self.foc_mode {
85      self.shaft_position_req = position;
86    }
87  }
88  /// acceleration in rad/sec2
89  pub fn set_acceleration(&mut self, acc: I16F16) {
90    self.speed_acc = acc / 100; // update is done with 100 hrz
91  }
92  /// only to be used for test function. Be carefull with setting the value manually!
93  /// Incorrect use can damage the motor.
94  pub fn set_electrical_offset(&mut self, offset: I16F16) {
95    self.electrical_offset = offset;
96  }
97
98  /// return the busy flag, to see if the calibration is finished, or the target angle is reached
99  pub fn is_idle(&self) -> bool {
100    self.foc_mode == EFocMode::Idle
101  }
102
103  pub fn get_velocity(&self) -> I16F16 {
104    self.velocity
105  }
106  pub fn get_position_act(&self) -> ShaftPosition {
107    self.shaft_position_act
108  }
109  pub fn get_position_req(&self) -> ShaftPosition {
110    self.shaft_position_req
111  }
112  pub fn set_position_req(&mut self, req: ShaftPosition) {
113    self.shaft_position_req = req;
114  }
115
116  pub fn set_foc_mode(&mut self, mode: EFocMode) -> Result<()> {
117    rprintln!("Set Foc mode");
118    self.foc_mode = mode;
119    match self.foc_mode {
120      EFocMode::Calibration(param) => match param {
121        Some(p) => {
122          self.electrical_offset = p.zero;
123          self.shaft_position_act.set_inversed(p.dir == EDir::Ccw);
124          self.foc_mode = EFocMode::Idle;
125        }
126        None => {
127          self.electrical_offset = I16F16::ZERO;
128          self.shaft_position_act.set_inversed(false);
129          self.calibration_state = ECalibrateState::Init;
130          if self.torque < I16F16::ONE/10 {
131            self.torque = I16F16::ONE/4;
132          }
133        }
134      },
135      EFocMode::Angle(param) => {
136        self.target_pid = FocPid::new(param.p, param.i, param.d);
137        self.target_pid.set_integral_max(I16F16::ONE * 3);
138
139        self.shaft_position_req = self.shaft_position_act.clone();
140      }
141      EFocMode::Velocity(param) => {
142        self.speed_req = I16F16::ZERO;
143        self.speed_act = I16F16::ZERO;
144        self.shaft_position_req = self.shaft_position_act.clone();
145        self.target_pid = FocPid::new(param.p, param.i, param.d);
146        self.target_pid.set_integral_max(I16F16::ONE * 20);
147      }
148      EFocMode::Torque(param) => {
149        self.torque = I16F16::ZERO;
150        self.target_pid = FocPid::new(param.p, param.i, param.d);
151        self.target_pid.set_integral_max(I16F16::ONE);
152      }
153      EFocMode::Idle => (),
154      EFocMode::Error(e) => return Err(e),
155    }
156    Ok(())
157  }
158
159  /// update the state of the foc controller. as fast as possible. Proposed value is each ms
160  /// Input is the measured shaft angle in radians
161  /// Details of how to get the angle are not in scope here, so no dependencies towards hardware
162  /// Returned is a tuple of the electrical angle, and the requested torque
163  pub fn update(&mut self, angle: EFocAngle) -> Result<(I16F16, I16F16)> {
164    let electrical_angle = match angle {
165      EFocAngle::SensorLess => I16F16::ZERO,
166      EFocAngle::SensorValue(angle) => {
167        self.shaft_position_act.update_shaft_angle(angle);
168        I16F16::from_num(self.nr_poles) * self.shaft_position_act.angle - self.electrical_offset
169      }
170      EFocAngle::Interpolate => {
171        I16F16::from_num(self.nr_poles) * self.shaft_position_act.angle - self.electrical_offset
172      }
173    };
174    match self.foc_mode {
175      EFocMode::Idle => Ok((electrical_angle, I16F16::ZERO)),
176      EFocMode::Error(e) => Err(e),
177      EFocMode::Calibration(_) => match angle {
178        EFocAngle::SensorLess => Err(EFocSimpleError::NoAngleSensor),
179        _ => self.do_calibration(),
180      },
181
182      EFocMode::Angle(_) => {
183        match angle {
184          EFocAngle::SensorLess => Err(EFocSimpleError::NoAngleSensor),
185          _ => {
186            // Compare actual position with requested
187            let torque = self
188              .target_pid
189              .update_position(&self.shaft_position_req, &self.shaft_position_act);
190            Ok((electrical_angle, torque))
191          }
192        }
193      }
194      EFocMode::Velocity(_) => {
195        match angle {
196          EFocAngle::SensorLess => {
197            // note that in sensorless mode the shaft_position_req is the electrical angle of the shaft
198            let delta_electrical_angle = I16F16::from_num(self.nr_poles) * self.speed_act / 1000;
199            self.shaft_position_req.inc(delta_electrical_angle); // increment requested angle in rad/s
200                                                                 // set the torque with the torque limit function
201            let request_angle = self.shaft_position_req.get_angle();
202            Ok((request_angle, I16F16::ONE / 4))
203          }
204          _ => {
205            // increment the requested shaft position, but only if the diff with the actual shaft position is not too big
206            let diff = self.shaft_position_req.compare(&self.shaft_position_act);
207            if diff.abs() < I16F16::PI {
208              let delta_angle = self.speed_act / 1000;
209              self.shaft_position_req.inc(delta_angle);
210            }
211            let requested_torque = self
212              .target_pid
213              .update_position(&self.shaft_position_req, &self.shaft_position_act);
214            Ok((electrical_angle, requested_torque))
215          }
216        }
217      }
218      EFocMode::Torque(_) => match angle {
219        EFocAngle::SensorLess => Err(EFocSimpleError::NoAngleSensor),
220        _ => Ok((electrical_angle, self.torque)),
221      },
222    }
223  }
224
225  /// calculate the velocity. This function should be called each 10 ms.
226  /// The low pass filter frequency is 10 hrz
227  pub fn update_velocity(&mut self, ts: usize) {
228    // update the actual requested speed  with a fixed frequency of preferable 100 hz
229    self.update_speed();
230    let delta_ts = ts - self.timestamp_vel;
231    if delta_ts > 0 {
232      self.timestamp_vel = ts;
233      let delta_sec = I16F16::from_num(delta_ts) / 1_000;
234      let position_delta = self.shaft_position_act.delta();
235      let velocity_current = position_delta / delta_sec; // in rad per second
236                                                         // filter the velocity with a low pass filter
237
238      self.velocity = (velocity_current + 19 * self.velocity) / 20;
239    }
240  }
241
242  #[inline]
243  fn update_speed(&mut self) {
244    let req = self.speed_req;
245    if self.speed_acc == 0 {
246      self.speed_act = req;
247    } else {
248      let mut act = self.speed_act;
249      if act > req {
250        act -= self.speed_acc;
251        if act < req {
252          act = req;
253        }
254      } else if act < req {
255        act += self.speed_acc;
256        if act > req {
257          act = req;
258        }
259      } // do nothing if equal
260      self.speed_act = act;
261    }
262  }
263
264  /// Calculate the direction of the sensor in relation to the direction of the motor
265  /// If needed invert the direction of the sensor
266  /// Returned is a tuple of electrical angle and torque
267  fn do_calibration(&mut self) -> Result<(I16F16, I16F16)> {
268    let req = self.shaft_position_req;
269    let act = self.shaft_position_act;
270
271    match self.calibration_state {
272      ECalibrateState::Init => {
273        self.shaft_position_req.reset();
274        self.shaft_position_act.reset();
275        self.electrical_offset = I16F16::ZERO;
276        self.calibration_state = ECalibrateState::FindDirection
277      }
278      ECalibrateState::FindDirection => {
279        // state end condition after exact 1 electrical turn
280        if req.rotations > 1 {
281          // check motor did move
282          if act.rotations == 0 && act.angle == I16F16::ZERO {
283            self.calibration_state = ECalibrateState::Init;
284            self.foc_mode = EFocMode::Error(EFocSimpleError::NoMotorMovement);
285            rprintln!("End condition No motor movement detected");
286          } else {
287            // set the direction
288            if act.get_position() < 0 {
289              // reverse the direction in the driver. Motor must run in the same dir as the sensor
290              rprintln!("Direction inversed");
291              self.shaft_position_act.set_inversed(true);
292            } else {
293              rprintln!("Direction not inversed");
294              self.shaft_position_act.set_inversed(false);
295            }
296
297            self.calibration_state = ECalibrateState::FindOffset;
298          }
299        } else {
300          // rotate with 10 rad/sec positive
301          self.shaft_position_req.inc(I16F16::from_num(0.01));
302        }
303      }
304      ECalibrateState::FindOffset => {
305        // state end condition after exact 2 electrical turns + 3/4 TAU
306        if req.rotations > 2 && req.angle > 3 * I16F16::FRAC_TAU_4 {
307          // determin electrical offset with current offset == 0
308          let offset = self.shaft_position_act.get_angle() * self.nr_poles as i32;
309          // normalize to 0 .. TAU
310          self.electrical_offset = ShaftPosition::clamp(offset);
311          self.calibration_state = ECalibrateState::ReturnToStart;
312          rprintln!("Electrical offset:{}", self.electrical_offset);
313        } else {
314          // rotate with 10 rad/sec positive
315          self.shaft_position_req.inc(I16F16::from_num(0.01));
316        }
317      }
318      ECalibrateState::ReturnToStart => {
319        // end conditions at start positon plu TAU/2
320        if req.rotations == 0 && req.angle < I16F16::FRAC_TAU_2 {
321          self.foc_mode = EFocMode::Idle;
322          rprintln!("Calibraton finished");
323        } else {
324          // rotate with 10 rad/sec negative
325          self.shaft_position_req.inc(I16F16::from_num(-0.01));
326        }
327      }
328    }
329    Ok((self.shaft_position_req.get_angle(), self.torque.abs()))
330  }
331}
foc_simple/foc/foc_simple.rs

foc_simple/foc/
foc_simple.rs
