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