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