Telescope Control Library
Diff: AMIS30543StepperDriver.cpp
- Revision:
- 0:6cb2eaf8b133
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AMIS30543StepperDriver.cpp Sun Aug 19 05:21:20 2018 +0000 @@ -0,0 +1,322 @@ +/* + * AMIS30543StepperDriver.cpp + * + * Created on: 2018��2��8�� + * Author: caoyuan9642 + */ + +#include <AMIS30543StepperDriver.h> +#include "mbed.h" +#include "pinmap.h" +AMIS30543StepperDriver::AMIS30543StepperDriver(SPI *spi, PinName cs, + PinName step, PinName dir, PinName err, bool invert) : + StepperMotor(invert), spi(spi), cs(cs, 1), step(step), dir(dir), err( + err), status(IDLE), inc(1), stepCount(0) +{ + if (dir != NC) + { + useDIR = true; + this->dir = 0; /*Forward*/ + } + else + useDIR = false; + if (err != NC) + { + useERR = true; + } + else + useERR = false; + + spi->frequency(400000); + + /*CPOL=0, CPHA=0*/ + spi->format(8, 0); + + wait_us(10); + + /*Perform initialization*/ + writeReg(CR0, 0x00); // 1/32 step + writeReg(CR1, 0x00); // + writeReg(CR2, 0x80); // MOTEN = 1 + writeReg(CR3, 0x00); // 1/32 step + + microstep = 32; +} + +AMIS30543StepperDriver::~AMIS30543StepperDriver() +{ + printf("AMIS Shutdown\n"); + writeReg(CR2, 0x00); // MOTEN = 0, stop driving the motor + writeReg(CR2, 0x00); // MOTEN = 0, stop driving the motor + writeReg(CR2, 0x00); // MOTEN = 0, stop driving the motor +} + +void AMIS30543StepperDriver::writeReg(regaddr_t addr, uint8_t data) +{ + assertCS(); + wait_us(3); + txbuf[0] = (char) (addr & 0x1F) | 0x80; + txbuf[1] = data; + spi->write(txbuf, 2, NULL, 0); + wait_us(3); + deassertCS(); + wait_us(6); +} + +uint8_t AMIS30543StepperDriver::readReg(regaddr_t addr) +{ + assertCS(); + wait_us(3); + txbuf[0] = (char) (addr & 0x1F); + spi->write(txbuf, 2, rxbuf, 2); + wait_us(3); + deassertCS(); + wait_us(6); + return rxbuf[1]; +} + +void AMIS30543StepperDriver::start(stepdir_t dir) +{ + /*Set Direction*/ + if (status == IDLE) + { + if (useDIR) + { + /*Hard switch*/ + if ((dir == STEP_FORWARD) ^ invert) + this->dir = 0; + else + this->dir = 1; + } + else + { + /*Soft switch*/ + uint8_t cr1 = readReg(CR1) & 0x7F; + if ((dir == STEP_FORWARD) ^ invert) + cr1 |= 0x00; + else + cr1 |= 0x80; + writeReg(CR1, cr1); + } + + inc = (dir == STEP_FORWARD) ? 1 : -1; + + // Start stepping + step.start(); + step.resetCount(); + status = STEPPING; + } +} + +void AMIS30543StepperDriver::stop() +{ + if (status == STEPPING) + { + status = IDLE; + step.stop(); + stepCount += ((double) step.getCount()) * inc / microstep; // the stepCount should be divided by microstep + } +} + +double AMIS30543StepperDriver::setFrequency(double frequency) +{ + return step.setFrequency(frequency * microstep) / microstep; //Use the microstepping frequency + +} + +double AMIS30543StepperDriver::getStepCount() +{ + if (status == IDLE) + return stepCount; + else + return stepCount + ((double) step.getCount()) * inc / microstep; // the step count should be divided by microstep +} + +void AMIS30543StepperDriver::setMicroStep(int microstep) +{ + uint8_t cr0 = readReg(CR0) & ~(0xE0); + + switch (microstep) + { + case 1: + writeReg(CR3, 0x03); // Compensated full-step + writeReg(CR0, cr0 | (0x00 << 5)); // 1/32-step + break; + case 2: + writeReg(CR3, 0x00); + writeReg(CR0, cr0 | (0x04 << 5)); // Compensated half-step + break; + case 4: + writeReg(CR3, 0x00); + writeReg(CR0, cr0 | (0x03 << 5)); // 1/4-step + break; + case 8: + writeReg(CR3, 0x00); + writeReg(CR0, cr0 | (0x02 << 5)); // 1/8-step + break; + case 16: + writeReg(CR3, 0x00); + writeReg(CR0, cr0 | (0x01 << 5)); // 1/16-step + break; + case 32: + writeReg(CR3, 0x00); + writeReg(CR0, cr0 | (0x00 << 5)); // 1/32-step + break; + case 64: + writeReg(CR3, 0x02); // 1/64-step + writeReg(CR0, cr0 | (0x00 << 5)); // 1/32-step + break; + case 128: + writeReg(CR3, 0x01); + writeReg(CR0, cr0 | (0x00 << 5)); // 1/32-step + break; + default: + debug("Error: microsteps must be a power of 2\n"); + return; + } + + // Update the microstep variable only if the value is valid + this->microstep = microstep; +} + +void AMIS30543StepperDriver::setCurrent(double current) +{ + uint8_t reg_cur = 0; + uint8_t cr0 = readReg(CR0) & ~(0x1F); + + if (current <= 0.132) + { + reg_cur = 0; + } + else if (current <= 0.245) + { + reg_cur = 1; + } + else if (current <= 0.355) + { + reg_cur = 2; + } + else if (current <= 0.395) + { + reg_cur = 3; + } + else if (current <= 0.445) + { + reg_cur = 4; + } + else if (current <= 0.485) + { + reg_cur = 5; + } + else if (current <= 0.540) + { + reg_cur = 6; + } + else if (current <= 0.585) + { + reg_cur = 7; + } + else if (current <= 0.640) + { + reg_cur = 8; + } + else if (current <= 0.715) + { + reg_cur = 9; + } + else if (current <= 0.780) + { + reg_cur = 10; + } + else if (current <= 0.870) + { + reg_cur = 11; + } + else if (current <= 0.955) + { + reg_cur = 12; + } + else if (current <= 1.060) + { + reg_cur = 13; + } + else if (current <= 1.150) + { + reg_cur = 14; + } + else if (current <= 1.260) + { + reg_cur = 15; + } + else if (current <= 1.405) + { + reg_cur = 16; + } + else if (current <= 1.520) + { + reg_cur = 17; + } + else if (current <= 1.695) + { + reg_cur = 18; + } + else if (current <= 1.820) + { + reg_cur = 19; + } + else if (current <= 2.070) + { + reg_cur = 20; + } + else if (current <= 2.240) + { + reg_cur = 21; + } + else if (current <= 2.440) + { + reg_cur = 22; + } + else if (current <= 2.700) + { + reg_cur = 23; + } + else if (current <= 2.845) + { + reg_cur = 24; + } + else if (current <= 3.000) + { + reg_cur = 25; + } + else + { + debug("Error: maximum current supported is 3.0A/phase\n"); + return; + } + writeReg(CR0, cr0 | reg_cur); +} + +inline void AMIS30543StepperDriver::assertCS() +{ + cs = 0; +} + +inline void AMIS30543StepperDriver::deassertCS() +{ + cs = 1; +} + +void AMIS30543StepperDriver::poweron() +{ + writeReg(CR2, 0x80); // MOTEN = 80, start driving the motor + writeReg(CR2, 0x80); // MOTEN = 80, start driving the motor + writeReg(CR2, 0x80); // MOTEN = 80, start driving the motor +} + +void AMIS30543StepperDriver::poweroff() +{ + writeReg(CR2, 0x00); // MOTEN = 0, stop driving the motor + writeReg(CR2, 0x00); // MOTEN = 0, stop driving the motor + writeReg(CR2, 0x00); // MOTEN = 0, stop driving the motor +} +