Commit | Line | Data |
---|---|---|
3eca6882 JM |
1 | #include "MotorDriverControl.h" |
2 | #include "libs/Kernel.h" | |
3 | #include "libs/nuts_bolts.h" | |
4 | #include "libs/utils.h" | |
5 | #include "ConfigValue.h" | |
6 | #include "libs/StreamOutput.h" | |
5b0cf319 | 7 | #include "libs/StreamOutputPool.h" |
fc320ac5 JM |
8 | #include "Robot.h" |
9 | #include "StepperMotor.h" | |
7b35a4c8 | 10 | #include "PublicDataRequest.h" |
45ef09fd | 11 | |
3eca6882 JM |
12 | #include "Gcode.h" |
13 | #include "Config.h" | |
14 | #include "checksumm.h" | |
3eca6882 | 15 | |
5b0cf319 | 16 | #include "mbed.h" // for SPI |
3eca6882 | 17 | |
45ef09fd | 18 | #include "drivers/TMC26X/TMC26X.h" |
e21e1f3a | 19 | #include "drivers/DRV8711/drv8711.h" |
45ef09fd | 20 | |
3eca6882 | 21 | #include <string> |
3eca6882 | 22 | |
5b0cf319 JM |
23 | #define motor_driver_control_checksum CHECKSUM("motor_driver_control") |
24 | #define enable_checksum CHECKSUM("enable") | |
25 | #define chip_checksum CHECKSUM("chip") | |
fc320ac5 | 26 | #define designator_checksum CHECKSUM("designator") |
5262f8d0 JM |
27 | #define alarm_checksum CHECKSUM("alarm") |
28 | #define halt_on_alarm_checksum CHECKSUM("halt_on_alarm") | |
5b0cf319 JM |
29 | |
30 | #define current_checksum CHECKSUM("current") | |
31 | #define max_current_checksum CHECKSUM("max_current") | |
5b0cf319 | 32 | |
45ef09fd | 33 | #define microsteps_checksum CHECKSUM("microsteps") |
5b0cf319 | 34 | #define decay_mode_checksum CHECKSUM("decay_mode") |
3eca6882 | 35 | |
7b35a4c8 | 36 | #define raw_register_checksum CHECKSUM("reg") |
3eca6882 | 37 | |
45ef09fd JM |
38 | #define spi_channel_checksum CHECKSUM("spi_channel") |
39 | #define spi_cs_pin_checksum CHECKSUM("spi_cs_pin") | |
40 | #define spi_frequency_checksum CHECKSUM("spi_frequency") | |
3eca6882 | 41 | |
fc320ac5 | 42 | MotorDriverControl::MotorDriverControl(uint8_t id) : id(id) |
3eca6882 | 43 | { |
0bfaf040 | 44 | enable_event= false; |
b01faaff JM |
45 | current_override= false; |
46 | microstep_override= false; | |
3eca6882 JM |
47 | } |
48 | ||
49 | MotorDriverControl::~MotorDriverControl() | |
50 | { | |
3eca6882 JM |
51 | } |
52 | ||
53 | // this will load all motor driver controls defined in config, called from main | |
54 | void MotorDriverControl::on_module_loaded() | |
55 | { | |
56 | vector<uint16_t> modules; | |
57 | THEKERNEL->config->get_module_list( &modules, motor_driver_control_checksum ); | |
fc320ac5 | 58 | uint8_t cnt = 1; |
3eca6882 | 59 | for( auto cs : modules ) { |
5b0cf319 | 60 | // If module is enabled create an instance and initialize it |
3eca6882 | 61 | if( THEKERNEL->config->value(motor_driver_control_checksum, cs, enable_checksum )->as_bool() ) { |
fc320ac5 JM |
62 | MotorDriverControl *controller = new MotorDriverControl(cnt++); |
63 | if(!controller->config_module(cs)) delete controller; | |
3eca6882 JM |
64 | } |
65 | } | |
66 | ||
67 | // we don't need this instance anymore | |
68 | delete this; | |
69 | } | |
70 | ||
fc320ac5 | 71 | bool MotorDriverControl::config_module(uint16_t cs) |
3eca6882 JM |
72 | { |
73 | spi_cs_pin.from_string(THEKERNEL->config->value( motor_driver_control_checksum, cs, spi_cs_pin_checksum)->by_default("nc")->as_string())->as_output(); | |
5b0cf319 JM |
74 | if(!spi_cs_pin.connected()) { |
75 | THEKERNEL->streams->printf("MotorDriverControl ERROR: chip select not defined\n"); | |
76 | return false; // if not defined then we can't use this instance | |
77 | } | |
45ef09fd JM |
78 | spi_cs_pin.set(1); |
79 | ||
fc320ac5 | 80 | std::string str= THEKERNEL->config->value( motor_driver_control_checksum, cs, designator_checksum)->by_default("")->as_string(); |
5b0cf319 JM |
81 | if(str.empty()) { |
82 | THEKERNEL->streams->printf("MotorDriverControl ERROR: designator not defined\n"); | |
83 | return false; // designator required | |
84 | } | |
3eca6882 JM |
85 | designator= str[0]; |
86 | ||
fc320ac5 | 87 | str= THEKERNEL->config->value( motor_driver_control_checksum, cs, chip_checksum)->by_default("")->as_string(); |
5b0cf319 JM |
88 | if(str.empty()) { |
89 | THEKERNEL->streams->printf("MotorDriverControl ERROR: chip type not defined\n"); | |
90 | return false; // chip type required | |
91 | } | |
92 | ||
45ef09fd JM |
93 | using std::placeholders::_1; |
94 | using std::placeholders::_2; | |
95 | using std::placeholders::_3; | |
96 | ||
5b0cf319 JM |
97 | if(str == "DRV8711") { |
98 | chip= DRV8711; | |
e21e1f3a | 99 | drv8711= new DRV8711DRV(std::bind( &MotorDriverControl::sendSPI, this, _1, _2, _3)); |
45ef09fd | 100 | |
5b0cf319 JM |
101 | }else if(str == "TMC2660") { |
102 | chip= TMC2660; | |
45ef09fd JM |
103 | tmc26x= new TMC26X(std::bind( &MotorDriverControl::sendSPI, this, _1, _2, _3)); |
104 | ||
5b0cf319 JM |
105 | }else{ |
106 | THEKERNEL->streams->printf("MotorDriverControl ERROR: Unknown chip type: %s\n", str.c_str()); | |
107 | return false; | |
108 | } | |
109 | ||
3eca6882 | 110 | // select which SPI channel to use |
5b0cf319 | 111 | int spi_channel = THEKERNEL->config->value(motor_driver_control_checksum, cs, spi_channel_checksum)->by_default(1)->as_number(); |
3eca6882 JM |
112 | int spi_frequency = THEKERNEL->config->value(motor_driver_control_checksum, cs, spi_frequency_checksum)->by_default(1000000)->as_number(); |
113 | ||
45ef09fd | 114 | // select SPI channel to use |
3eca6882 JM |
115 | PinName mosi, miso, sclk; |
116 | if(spi_channel == 0) { | |
117 | mosi = P0_18; miso = P0_17; sclk = P0_15; | |
118 | } else if(spi_channel == 1) { | |
119 | mosi = P0_9; miso = P0_8; sclk = P0_7; | |
120 | } else { | |
5b0cf319 JM |
121 | THEKERNEL->streams->printf("MotorDriverControl ERROR: Unknown SPI Channel: %d\n", spi_channel); |
122 | return false; | |
3eca6882 JM |
123 | } |
124 | ||
125 | this->spi = new mbed::SPI(mosi, miso, sclk); | |
126 | this->spi->frequency(spi_frequency); | |
fc320ac5 | 127 | this->spi->format(8, 3); // 8bit, mode3 |
3eca6882 | 128 | |
c611d712 JM |
129 | // set default max currents for each chip, can be overidden in config |
130 | switch(chip) { | |
131 | case DRV8711: max_current= 4000; break; | |
132 | case TMC2660: max_current= 4000; break; | |
133 | } | |
134 | ||
135 | max_current= THEKERNEL->config->value(motor_driver_control_checksum, cs, max_current_checksum )->by_default((int)max_current)->as_number(); // in mA | |
7b0f0de5 | 136 | //current_factor= THEKERNEL->config->value(motor_driver_control_checksum, cs, current_factor_checksum )->by_default(1.0F)->as_number(); |
5b0cf319 | 137 | |
7b0f0de5 JM |
138 | current= THEKERNEL->config->value(motor_driver_control_checksum, cs, current_checksum )->by_default(1000)->as_number(); // in mA |
139 | microsteps= THEKERNEL->config->value(motor_driver_control_checksum, cs, microsteps_checksum )->by_default(16)->as_number(); // 1/n | |
df16e9b0 | 140 | //decay_mode= THEKERNEL->config->value(motor_driver_control_checksum, cs, decay_mode_checksum )->by_default(1)->as_number(); |
5b0cf319 | 141 | |
45ef09fd JM |
142 | // setup the chip via SPI |
143 | initialize_chip(); | |
3eca6882 | 144 | |
7b35a4c8 JM |
145 | // if raw registers are defined set them 1,2,3 etc in hex |
146 | str= THEKERNEL->config->value( motor_driver_control_checksum, cs, raw_register_checksum)->by_default("")->as_string(); | |
147 | if(!str.empty()) { | |
148 | rawreg= true; | |
149 | std::vector<uint32_t> regs= parse_number_list(str.c_str(), 16); | |
30faab4a JM |
150 | if(!regs.empty()) { |
151 | uint32_t reg= 0; | |
152 | for(auto i : regs) { | |
153 | // this just sets the local storage, it does not write to the chip | |
154 | switch(chip) { | |
155 | case DRV8711: drv8711->set_raw_register(&StreamOutput::NullStream, ++reg, i); break; | |
156 | case TMC2660: tmc26x->setRawRegister(&StreamOutput::NullStream, ++reg, i); break; | |
157 | } | |
158 | } | |
159 | ||
160 | // write the stored registers | |
7b35a4c8 | 161 | switch(chip) { |
30faab4a JM |
162 | case DRV8711: drv8711->set_raw_register(&StreamOutput::NullStream, 255, 0); break; |
163 | case TMC2660: tmc26x->setRawRegister(&StreamOutput::NullStream, 255, 0); break; | |
7b35a4c8 JM |
164 | } |
165 | } | |
30faab4a | 166 | |
7b35a4c8 JM |
167 | }else{ |
168 | rawreg= false; | |
169 | } | |
170 | ||
3eca6882 | 171 | this->register_for_event(ON_GCODE_RECEIVED); |
7b35a4c8 | 172 | this->register_for_event(ON_HALT); |
0bfaf040 JM |
173 | this->register_for_event(ON_ENABLE); |
174 | this->register_for_event(ON_IDLE); | |
fc320ac5 | 175 | |
5262f8d0 JM |
176 | if( THEKERNEL->config->value(motor_driver_control_checksum, cs, alarm_checksum )->by_default(false)->as_bool() ) { |
177 | halt_on_alarm= THEKERNEL->config->value(motor_driver_control_checksum, cs, halt_on_alarm_checksum )->by_default(false)->as_bool(); | |
178 | // enable alarm monitoring for the chip | |
179 | this->register_for_event(ON_SECOND_TICK); | |
180 | } | |
181 | ||
fc320ac5 JM |
182 | THEKERNEL->streams->printf("MotorDriverControl INFO: configured motor %c (%d): as %s, cs: %04X\n", designator, id, chip==TMC2660?"TMC2660":chip==DRV8711?"DRV8711":"UNKNOWN", (spi_cs_pin.port_number<<8)|spi_cs_pin.pin); |
183 | ||
3eca6882 JM |
184 | return true; |
185 | } | |
186 | ||
0bfaf040 JM |
187 | // event to handle enable on/off, as it could be called in an ISR we schedule to turn the steppers on or off in ON_IDLE |
188 | // This may cause the initial step to be missed if on-idle is delayed too much but we can't do SPI in an interrupt | |
189 | void MotorDriverControl::on_enable(void *argument) | |
190 | { | |
191 | enable_event= true; | |
192 | enable_flg= (argument != nullptr); | |
193 | } | |
194 | ||
195 | void MotorDriverControl::on_idle(void *argument) | |
196 | { | |
197 | if(enable_event) { | |
198 | enable_event= false; | |
199 | enable(enable_flg); | |
200 | } | |
201 | } | |
202 | ||
7b35a4c8 | 203 | void MotorDriverControl::on_halt(void *argument) |
45ef09fd | 204 | { |
7b35a4c8 JM |
205 | if(argument == nullptr) { |
206 | enable(false); | |
45ef09fd JM |
207 | } |
208 | } | |
209 | ||
5ca112ee | 210 | // runs in on_idle, does SPI transaction |
5262f8d0 JM |
211 | void MotorDriverControl::on_second_tick(void *argument) |
212 | { | |
d74b7399 JM |
213 | // we don't want to keep checking once we have been halted by an error |
214 | if(THEKERNEL->is_halted()) return; | |
215 | ||
5262f8d0 JM |
216 | bool alarm=false;; |
217 | switch(chip) { | |
218 | case DRV8711: | |
219 | alarm= drv8711->check_alarm(); | |
220 | break; | |
221 | ||
222 | case TMC2660: | |
223 | alarm= tmc26x->checkAlarm(); | |
224 | break; | |
225 | } | |
226 | ||
227 | if(halt_on_alarm && alarm) { | |
228 | THEKERNEL->call_event(ON_HALT, nullptr); | |
229 | THEKERNEL->streams->printf("Motor Driver alarm - reset or M999 required to continue\r\n"); | |
230 | } | |
231 | } | |
232 | ||
3eca6882 JM |
233 | void MotorDriverControl::on_gcode_received(void *argument) |
234 | { | |
235 | Gcode *gcode = static_cast<Gcode*>(argument); | |
236 | ||
237 | if (gcode->has_m) { | |
45ef09fd | 238 | if(gcode->m == 906) { |
7b0f0de5 | 239 | if (gcode->has_letter(designator)) { |
45ef09fd | 240 | // set motor currents in mA (Note not using M907 as digipots use that) |
3eca6882 | 241 | current= gcode->get_value(designator); |
7b0f0de5 | 242 | current= std::min(current, max_current); |
3eca6882 | 243 | set_current(current); |
b01faaff | 244 | current_override= true; |
3eca6882 JM |
245 | } |
246 | ||
fc320ac5 | 247 | } else if(gcode->m == 909) { // M909 Annn set microstepping, M909.1 also change steps/mm |
5b0cf319 | 248 | if (gcode->has_letter(designator)) { |
fc320ac5 | 249 | uint32_t current_microsteps= microsteps; |
5b0cf319 | 250 | microsteps= gcode->get_value(designator); |
fc320ac5 JM |
251 | microsteps= set_microstep(microsteps); // driver may change the steps it sets to |
252 | if(gcode->subcode == 1 && current_microsteps != microsteps) { | |
253 | // also reset the steps/mm | |
254 | int a= designator-'A'; | |
255 | if(a >= 0 && a <=2) { | |
256 | float s= THEKERNEL->robot->actuators[a]->get_steps_per_mm()*((float)microsteps/current_microsteps); | |
257 | THEKERNEL->robot->actuators[a]->change_steps_per_mm(s); | |
258 | gcode->stream->printf("steps/mm for %c changed to: %f\n", designator, s); | |
3c9fee28 | 259 | THEKERNEL->robot->check_max_actuator_speeds(); |
fc320ac5 JM |
260 | } |
261 | } | |
b01faaff | 262 | microstep_override= true; |
5b0cf319 JM |
263 | } |
264 | ||
df16e9b0 JM |
265 | // } else if(gcode->m == 910) { // set decay mode |
266 | // if (gcode->has_letter(designator)) { | |
267 | // decay_mode= gcode->get_value(designator); | |
268 | // set_decay_mode(decay_mode); | |
269 | // } | |
5b0cf319 | 270 | |
7b0f0de5 | 271 | } else if(gcode->m == 911) { |
df16e9b0 | 272 | // set or get raw registers |
6e4b6254 | 273 | // M911 will dump all the registers and status of all the motors |
eab91f11 | 274 | // M911.1 Pn (or A0) will dump the registers and status of the selected motor. X0 will request format in processing machine readable format |
df16e9b0 | 275 | // M911.2 Pn (or B0) Rxxx Vyyy sets Register xxx to value yyy for motor nnn, xxx == 255 writes the registers, xxx == 0 shows what registers are mapped to what |
33483c6f JM |
276 | // M911.3 Pn (or C0) will set the options based on the parameters passed as below... |
277 | // TMC2660:- | |
278 | // M911.3 Onnn Qnnn setStallGuardThreshold O=stall_guard_threshold, Q=stall_guard_filter_enabled | |
33483c6f | 279 | // M911.3 Hnnn Innn Jnnn Knnn Lnnn setCoolStepConfiguration H=lower_SG_threshold, I=SG_hysteresis, J=current_decrement_step_size, K=current_increment_step_size, L=lower_current_limit |
6e4b6254 JM |
280 | // M911.3 S0 Unnn Vnnn Wnnn Xnnn Ynnn setConstantOffTimeChopper U=constant_off_time, V=blank_time, W=fast_decay_time_setting, X=sine_wave_offset, Y=use_current_comparator |
281 | // M911.3 S1 Unnn Vnnn Wnnn Xnnn Ynnn setSpreadCycleChopper U=constant_off_time, V=blank_time, W=hysteresis_start, X=hysteresis_end, Y=hysteresis_decrement | |
282 | // M911.3 S2 Zn setRandomOffTime Z=on|off Z1 is on Z0 is off | |
283 | // M911.3 S3 Zn setDoubleEdge Z=on|off Z1 is on Z0 is off | |
284 | // M911.3 S4 Zn setStepInterpolation Z=on|off Z1 is on Z0 is off | |
b023850b | 285 | // M911.3 S5 Zn setCoolStepEnabled Z=on|off Z1 is on Z0 is off |
33483c6f | 286 | |
df16e9b0 JM |
287 | if(gcode->subcode == 0 && gcode->get_num_args() == 0) { |
288 | // M911 no args dump status for all drivers, M911.1 P0|A0 dump for specific driver | |
7b0f0de5 | 289 | gcode->stream->printf("Motor %d (%c)...\n", id, designator); |
eab91f11 | 290 | dump_status(gcode->stream, true); |
7b0f0de5 | 291 | |
df16e9b0 JM |
292 | }else if(gcode->get_value('P') == id || gcode->has_letter(designator)) { |
293 | if(gcode->subcode == 1) { | |
eab91f11 | 294 | dump_status(gcode->stream, !gcode->has_letter('X')); |
df16e9b0 JM |
295 | |
296 | }else if(gcode->subcode == 2 && gcode->has_letter('R') && gcode->has_letter('V')) { | |
297 | set_raw_register(gcode->stream, gcode->get_value('R'), gcode->get_value('V')); | |
298 | ||
299 | }else if(gcode->subcode == 3 ) { | |
300 | set_options(gcode); | |
301 | } | |
7b0f0de5 JM |
302 | } |
303 | ||
3eca6882 | 304 | } else if(gcode->m == 500 || gcode->m == 503) { |
b01faaff JM |
305 | if(current_override) { |
306 | gcode->stream->printf(";Motor %c id %d current mA:\n", designator, id); | |
307 | gcode->stream->printf("M906 %c%lu\n", designator, current); | |
308 | } | |
309 | if(microstep_override) { | |
310 | gcode->stream->printf(";Motor %c id %d microsteps:\n", designator, id); | |
311 | gcode->stream->printf("M909 %c%lu\n", designator, microsteps); | |
312 | } | |
df16e9b0 | 313 | //gcode->stream->printf("M910 %c%d\n", designator, decay_mode); |
b01faaff | 314 | } |
3eca6882 JM |
315 | } |
316 | } | |
317 | ||
45ef09fd JM |
318 | void MotorDriverControl::initialize_chip() |
319 | { | |
320 | // send initialization sequence to chips | |
321 | if(chip == DRV8711) { | |
fbeb173d JM |
322 | drv8711->init(); |
323 | set_current(current); | |
324 | set_microstep(microsteps); | |
45ef09fd JM |
325 | |
326 | }else if(chip == TMC2660){ | |
45ef09fd JM |
327 | tmc26x->init(); |
328 | set_current(current); | |
e21e1f3a | 329 | set_microstep(microsteps); |
fbeb173d | 330 | //set_decay_mode(decay_mode); |
45ef09fd JM |
331 | } |
332 | ||
45ef09fd JM |
333 | } |
334 | ||
e21e1f3a | 335 | // set current in milliamps |
45ef09fd | 336 | void MotorDriverControl::set_current(uint32_t c) |
3eca6882 | 337 | { |
5b0cf319 | 338 | switch(chip) { |
e21e1f3a | 339 | case DRV8711: |
fbeb173d | 340 | drv8711->set_current(c); |
e21e1f3a | 341 | break; |
45ef09fd JM |
342 | |
343 | case TMC2660: | |
344 | tmc26x->setCurrent(c); | |
345 | break; | |
5b0cf319 JM |
346 | } |
347 | } | |
348 | ||
45ef09fd | 349 | // set microsteps where n is the number of microsteps eg 64 for 1/64 |
fc320ac5 | 350 | uint32_t MotorDriverControl::set_microstep( uint32_t n ) |
5b0cf319 | 351 | { |
fc320ac5 | 352 | uint32_t m= n; |
5b0cf319 | 353 | switch(chip) { |
e21e1f3a | 354 | case DRV8711: |
fbeb173d | 355 | m= drv8711->set_microsteps(n); |
e21e1f3a | 356 | break; |
45ef09fd JM |
357 | |
358 | case TMC2660: | |
359 | tmc26x->setMicrosteps(n); | |
fc320ac5 | 360 | m= tmc26x->getMicrosteps(); |
45ef09fd | 361 | break; |
5b0cf319 | 362 | } |
fc320ac5 | 363 | return m; |
5b0cf319 JM |
364 | } |
365 | ||
0bfaf040 | 366 | // TODO how to handle this? SO many options |
5b0cf319 JM |
367 | void MotorDriverControl::set_decay_mode( uint8_t dm ) |
368 | { | |
369 | switch(chip) { | |
370 | case DRV8711: break; | |
371 | case TMC2660: break; | |
372 | } | |
373 | } | |
374 | ||
45ef09fd JM |
375 | void MotorDriverControl::enable(bool on) |
376 | { | |
377 | switch(chip) { | |
e21e1f3a JM |
378 | case DRV8711: |
379 | drv8711->set_enable(on); | |
380 | break; | |
381 | ||
45ef09fd JM |
382 | case TMC2660: |
383 | tmc26x->setEnabled(on); | |
384 | break; | |
385 | } | |
386 | } | |
387 | ||
eab91f11 | 388 | void MotorDriverControl::dump_status(StreamOutput *stream, bool b) |
45ef09fd JM |
389 | { |
390 | switch(chip) { | |
e21e1f3a JM |
391 | case DRV8711: |
392 | drv8711->dump_status(stream); | |
393 | break; | |
394 | ||
45ef09fd | 395 | case TMC2660: |
eab91f11 | 396 | tmc26x->dumpStatus(stream, b); |
45ef09fd JM |
397 | break; |
398 | } | |
399 | } | |
df16e9b0 | 400 | |
7b0f0de5 JM |
401 | void MotorDriverControl::set_raw_register(StreamOutput *stream, uint32_t reg, uint32_t val) |
402 | { | |
403 | bool ok= false; | |
404 | switch(chip) { | |
5262f8d0 | 405 | case DRV8711: ok= drv8711->set_raw_register(stream, reg, val); break; |
7b0f0de5 JM |
406 | case TMC2660: ok= tmc26x->setRawRegister(stream, reg, val); break; |
407 | } | |
408 | if(ok) { | |
df16e9b0 | 409 | stream->printf("register operation succeeded\n"); |
7b0f0de5 JM |
410 | }else{ |
411 | stream->printf("register operation failed\n"); | |
412 | } | |
413 | } | |
45ef09fd | 414 | |
df16e9b0 JM |
415 | void MotorDriverControl::set_options(Gcode *gcode) |
416 | { | |
417 | switch(chip) { | |
418 | case DRV8711: break; | |
419 | ||
420 | case TMC2660: { | |
421 | TMC26X::options_t options= gcode->get_args_int(); | |
422 | if(options.size() > 0) { | |
423 | if(tmc26x->set_options(options)) { | |
424 | gcode->stream->printf("options set\n"); | |
425 | }else{ | |
426 | gcode->stream->printf("failed to set any options\n"); | |
427 | } | |
428 | } | |
429 | // options.clear(); | |
430 | // if(tmc26x->get_optional(options)) { | |
431 | // // foreach optional value | |
432 | // for(auto &i : options) { | |
433 | // // print all current values of supported options | |
434 | // gcode->stream->printf("%c: %d ", i.first, i.second); | |
435 | // gcode->add_nl = true; | |
436 | // } | |
437 | // } | |
438 | } | |
439 | break; | |
440 | } | |
441 | } | |
442 | ||
0bfaf040 | 443 | // Called by the drivers codes to send and receive SPI data to/from the chip |
45ef09fd JM |
444 | int MotorDriverControl::sendSPI(uint8_t *b, int cnt, uint8_t *r) |
445 | { | |
446 | spi_cs_pin.set(0); | |
447 | for (int i = 0; i < cnt; ++i) { | |
448 | r[i]= spi->write(b[i]); | |
449 | } | |
450 | spi_cs_pin.set(1); | |
451 | return cnt; | |
452 | } | |
453 |