src/modules/utils/motordrivercontrol/MotorDriverControl.cpp

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