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

foc_simple/tools/
foc_command.rs
