foc_simple/tools/
foc_command.rs

1use core::fmt::Write;
2use fixed::types::I16F16;
3use heapless::String;
4
5#[cfg(feature = "embassy")]
6use crate::foc::foc_embassy::FocSimpleCommand;
7#[cfg(feature = "rtic")]
8use crate::foc::foc_rtic::FocSimpleCommand;
9
10use crate::{
11  foc::{CalParams, EDir, EFocMode},
12  EFocCommand, FocParam, FocSerial,
13};
14
15#[derive(Clone, Debug, Copy, PartialEq)]
16#[repr(usize)]
17enum EFocModeLocal {
18  Idle = 0,
19  Calibration,
20  Velocity,
21  Angle,
22  Torque,
23}
24
25pub struct FocCommand<C: FocSerial> {
26  // user request
27  serial: C,
28  motor_nr: usize,
29  foc_mode: EFocModeLocal,
30  param_v: FocParam,
31  param_a: FocParam,
32  param_t: FocParam,
33  param_c: Option<CalParams>,
34}
35
36impl<C> FocCommand<C>
37where
38  C: FocSerial,
39{
40  pub fn new(serial: C) -> Self {
41    let param_v = FocParam::new_fp(0.1, 0.0, 0.0);
42    let param_a = FocParam::new_fp(0.4, 0.0, 0.0);
43    let param_t = FocParam::new_fp(1.0, 0.0, 0.0);
44    FocCommand {
45      serial,
46      foc_mode: EFocModeLocal::Idle,
47      param_a,
48      param_t,
49      param_v,
50      param_c: None,
51      motor_nr: 0,
52    }
53  }
54  /// the task will become the owner of Self
55  pub async fn task(&mut self) {
56    self.send("\r\nStarting Foc Serial Parser task\r\n");
57    self.help();
58    let mut line = [0_u8; 128];
59    let mut line_idx = 0;
60    loop {
61      let mut word = [0_u8; 32];
62      if let Ok(len) = self.serial.receive(&mut word).await {
63        for idx in 0..len {
64          let a = word[idx];
65          if a == 10 || a == 13 || a == 32 {
66            self.handle_line(&line[0..line_idx]);
67            line_idx = 0;
68          } else {
69            line[line_idx] = a;
70            line_idx += 1;
71          }
72        }
73      }
74    }
75  }
76
77  fn handle_line(&mut self, line: &[u8]) {
78    let gotmsg: &str = core::str::from_utf8(line).unwrap().into();
79    let words = gotmsg.split(" ");
80    for word in words {
81      self.handle_word(word);
82    }
83  }
84  fn handle_word(&mut self, m: &str) {
85    if m.len() < 2 {
86      self.send("Word must be at least 2 characters\r\n");
87      return;
88    }
89    match &m[0..2] {
90      "he" => self.help(),
91      "ts" => self.set_speed(m, "Speed:"),
92      "ta" => self.set_angle(m, "Angle:"),
93      "tt" => self.set_torque(m, "Torque:"),
94      "tl" => self.set_torque_limit(m, "Torque Limit"),
95      "pa" => self.set_acceleration(m, "Speed Acceleration:"),
96      "mi" => self.select_mode(EFocModeLocal::Idle),
97      "mc" => self.select_mode(EFocModeLocal::Calibration),
98      "mv" => self.select_mode(EFocModeLocal::Velocity),
99      "ma" => self.select_mode(EFocModeLocal::Angle),
100      "mt" => self.select_mode(EFocModeLocal::Torque),
101      "m0" => self.select_motor(0),
102      "m1" => self.select_motor(1),
103      "m2" => self.select_motor(2),
104      "kp" => self.set_param(&m),
105      "ki" => self.set_param(&m),
106      "kd" => self.set_param(&m),
107      "co" => self.set_calibration_offset(&m),
108      "ec" => FocSimpleCommand::send_command(self.motor_nr, EFocCommand::ErrorCount),
109      _ => self.help(),
110    }
111  }
112
113  fn send(&mut self, message: &str) {
114    let bytes = message.as_bytes();
115    _ = self.serial.send(bytes);
116  }
117
118  fn send_nl(&mut self) {
119    let bytes = "\r\n".as_bytes();
120    _ = self.serial.send(bytes);
121  }
122
123  pub fn send_usize(&mut self, data: usize) {
124    let mut msg: String<10> = String::new();
125    core::write!(&mut msg, "{}", data).unwrap();
126    self.send(&msg);
127  } /*
128    pub fn send_I16F16(&mut self, data: I16F16) {
129      let mut msg: String<20> = String::new();
130      core::write!(&mut msg, "{}", data).unwrap();
131      self.send(&msg);
132    }*/
133  pub fn send_i16f16(&mut self, data: I16F16) {
134    let mut msg: String<20> = String::new();
135    //let data: f32 = data.to_num();
136    core::write!(&mut msg, "{}", data).unwrap();
137    self.send(&msg);
138  }
139
140  fn select_mode(&mut self, mode: EFocModeLocal) {
141    self.send("Select mode:");
142    self.foc_mode = mode;
143    match mode {
144      EFocModeLocal::Angle => self.send("Angle\r\n"),
145      EFocModeLocal::Velocity => self.send("Velocity\r\n"),
146      EFocModeLocal::Torque => self.send("Torque\r\n"),
147      EFocModeLocal::Calibration => self.send("Calibration\r\n"),
148      EFocModeLocal::Idle => self.send("Idle\r\n"),
149    };
150    match mode {
151      EFocModeLocal::Angle => FocSimpleCommand::send_command(self.motor_nr, EFocCommand::FocMode(EFocMode::Angle(self.param_a))),
152      EFocModeLocal::Velocity => FocSimpleCommand::send_command(self.motor_nr, EFocCommand::FocMode(EFocMode::Velocity(self.param_v))),
153      EFocModeLocal::Torque => FocSimpleCommand::send_command(self.motor_nr, EFocCommand::FocMode(EFocMode::Torque(self.param_t))),
154      EFocModeLocal::Calibration => FocSimpleCommand::send_command(self.motor_nr, EFocCommand::FocMode(EFocMode::Calibration(None))),
155      EFocModeLocal::Idle => FocSimpleCommand::send_command(self.motor_nr, EFocCommand::FocMode(EFocMode::Idle)),
156    };
157  }
158  fn select_motor(&mut self, m: usize) {
159    self.send("Select motor:");
160    self.send_usize(m);
161    self.send_nl();
162    self.motor_nr = m;
163  }
164  fn set_speed(&mut self, word: &str, text: &str) {
165    if let Some(f) = self.parse_float(word, text) {
166      FocSimpleCommand::send_command(self.motor_nr, EFocCommand::Speed(f));
167    }
168  }
169  fn set_angle(&mut self, word: &str, text: &str) {
170    if let Some(f) = self.parse_float(word, text) {
171      FocSimpleCommand::send_command(self.motor_nr, EFocCommand::Angle(f));
172    }
173  }
174  fn set_torque(&mut self, word: &str, text: &str) {
175    if let Some(f) = self.parse_float(word, text) {
176      FocSimpleCommand::send_command(self.motor_nr, EFocCommand::Torque(f));
177    }
178  }
179  fn set_acceleration(&mut self, word: &str, text: &str) {
180    if let Some(f) = self.parse_float(word, text) {
181      FocSimpleCommand::send_command(self.motor_nr, EFocCommand::SpeedAcc(f));
182    }
183  }
184
185  fn set_torque_limit(&mut self, word: &str, text: &str) {
186    if let Some(f) = self.parse_float(word, text) {
187      FocSimpleCommand::send_command(self.motor_nr, EFocCommand::TorqueLimit(f));
188    }
189  }
190
191
192  fn parse_float(&mut self, word: &str, text: &str) -> Option<I16F16> {
193    self.send(text);
194    match word[2..].parse::<I16F16>() {
195      Err(_) => {
196        self.send("Unable to parse float after command\r\n");
197        None
198      }
199      Ok(f) => {
200        self.send_i16f16(f);
201        self.send_nl();
202        Some(f)
203      }
204    }
205  }
206
207  fn set_param(&mut self, word: &str) {
208    match word[2..].parse::<I16F16>() {
209      Err(_) => self.send("Unable to parse float after command\r\n"),
210      Ok(f) => {
211        let mode = match self.foc_mode {
212          EFocModeLocal::Angle => "Angle",
213          EFocModeLocal::Velocity => "Velocity",
214          EFocModeLocal::Torque => "Torque",
215          _ => "Invalid, choose mode first",
216        };
217        let p = match self.foc_mode {
218          EFocModeLocal::Angle => &mut self.param_a,
219          EFocModeLocal::Velocity => &mut self.param_v,
220          EFocModeLocal::Torque => &mut self.param_t,
221          _ => &mut FocParam::new(I16F16::ZERO, I16F16::ZERO, I16F16::ZERO),
222        };
223        match &word[0..2] {
224          "kp" => p.set_p(f),
225          "ki" => p.set_i(f),
226          "kd" => p.set_d(f),
227          _ => (),
228        };
229        self.send("Set pid parameters for mode:");
230        self.send(mode);
231        self.send(" ");
232        self.send(&word[0..2]);
233        self.send(" = ");
234        self.send_i16f16(f);
235        self.send_nl();
236      }
237    }
238  }
239
240  fn set_calibration_offset(&mut self, word: &str) {
241    match word[2..].parse::<I16F16>() {
242      Err(_) => self.send("Unable to parse float after command\r\n"),
243      Ok(f) => {
244        self.send("Set electrical offset to:");
245        self.send_i16f16(f);
246        self.send_nl();
247        self.param_c = Some(CalParams::new(EDir::Cw, I16F16::from_num(f)));
248        FocSimpleCommand::send_command(self.motor_nr, EFocCommand::FocMode(EFocMode::Calibration(self.param_c.clone())));
249      }
250    }
251  }
252
253  pub fn help(&mut self) {
254    self.send("\r\nHow to use  ... \r\n");
255    self.send("  he        -- help this message\r\n");
256    self.send("  ts<float> -- set target speed\r\n");
257    self.send("  ta<float> -- set target angle\r\n");
258    self.send("  tt<float> -- set target torque\r\n");
259    self.send("  tl<float> -- set max torque limit\r\n");
260    self.send("  pa<float> -- set speed acceleration in rad/sec2\r\n");
261    self.send("  mi        -- mode idle\r\n");
262    self.send("  mv        -- mode velocity\r\n");
263    self.send("  mt        -- mode torque\r\n");
264    self.send("  m0        -- select motor 0\r\n");
265    self.send("  m1        -- select motor 1\r\n");
266    self.send("  co        -- calibration offset. \r\n");
267    self.send("  pi<0|1>   -- enable/disable interpolation. \r\n");
268    self.send("  kp<float> -- pid P\r\n");
269    self.send("  ki<float> -- pid I\r\n");
270    self.send("  kd<float> -- pid D\r\n");
271    self.send("  ec        -- Show error count on rtt channel and reset\r\n");
272  }
273}
foc_simple/tools/foc_command.rs

foc_simple/tools/
foc_command.rs
