h3n3/darkwater_cplus_640
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

initial functions for stepper

Browse Source
This commit is contained in:
shrkey
2016-10-17 14:38:04 +01:00
parent 60113fcda9
commit 3d4679936e
2 changed files with 161 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

171
darkwater/DW640.cpp
View File

@@ -1,6 +1,6 @@
/*
Dark Water 640 driver code is placed under the BSD license.
Written by Team Dark Water (team@darkwater.io)
Written by Team Dark Water (team@darkwater.io) based off libraries by Adafruit https://github.com/adafruit/Adafruit_Motor_Shield_V2_Library
Copyright (c) 2014, Dark Water
All rights reserved.
@@ -316,12 +316,15 @@ DW_Stepper *DW640::getStepper(uint16_t steps, uint8_t stepper) {
// not init'd yet!
steppers[stepper].stepper = stepper;
steppers[stepper].revsteps = steps;
steppers[stepper].MC = this;
steppers[stepper].DWC = this;
steppers[stepper].AIN1pin = ain1;
steppers[stepper].AIN2pin = ain2;
steppers[stepper].BIN1pin = bin1;
steppers[stepper].BIN2pin = bin2;
// We need to set the board mode to ININ now that we have a stepper in existence
setMode(DW_ININ);
}
return &steppers[stepper];
}
@@ -421,21 +424,167 @@ void DW_Servo::setPWMuS(float length_uS) {
/* Stepper functions */
// void DW640::setStepperOff(uint8_t stepper) {
DW_Stepper::DW_Stepper(void) {
revsteps = stepper = currentstep = 0;
}
// }
void DW_Stepper::setMotorSpeed(uint16_t rpm) {
// void DW640::setStepperSpeed(uint8_t stepper, uint16_t speed) {
usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm);
}
// }
void DW_Stepper::off(void) {
DWC->setPin(AIN1pin, 1);
DWC->setPin(AIN2pin, 1);
DWC->setPin(BIN1pin, 1);
DWC->setPin(BIN2pin, 1);
}
// void DW640::oneStep(uint8_t stepper, uint8_t direction, uint8_t style) {
void DW_Stepper::step(uint16_t steps, uint8_t dir, uint8_t style) {
uint32_t uspers = usperstep;
uint8_t ret = 0;
// }
if (style == MICROSTEP) {
uspers /= MICROSTEPS;
steps *= MICROSTEPS;
}
// void DW640::step(uint8_t stepper, uint16_t steps, uint8_t direction, uint8_t style) {
while (steps--) {
ret = onestep(dir, style);
delayMicroseconds(uspers);
}
}
// }
uint8_t DW_Stepper::onestep(uint8_t dir, uint8_t style) {
uint8_t a, b, c, d;
uint8_t ocrb, ocra;
/* Private functions */
ocra = ocrb = 255; // We're not using these for now
// next determine what sort of stepping procedure we're up to
if (style == DW_SINGLE) {
if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird
if (dir == DW_FORWARD) {
currentstep += MICROSTEPS/2;
} else {
currentstep -= MICROSTEPS/2;
}
} else { // go to the next even step
if (dir == DW_FORWARD) {
currentstep += MICROSTEPS;
} else {
currentstep -= MICROSTEPS;
}
}
} else if (style == DW_DOUBLE) {
if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird
if (dir == DW_FORWARD) {
currentstep += MICROSTEPS/2;
} else {
currentstep -= MICROSTEPS/2;
}
} else { // go to the next odd step
if (dir == DW_FORWARD) {
currentstep += MICROSTEPS;
} else {
currentstep -= MICROSTEPS;
}
}
}
// Not tested this bit for now
if (style == DW_MICROSTEP) {
if (dir == DW_FORWARD) {
currentstep++;
} else {
// BACKWARDS
currentstep--;
}
currentstep += MICROSTEPS*4;
currentstep %= MICROSTEPS*4;
ocra = ocrb = 0;
if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) {
ocra = microstepcurve[MICROSTEPS - currentstep];
ocrb = microstepcurve[currentstep];
} else if ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) {
ocra = microstepcurve[currentstep - MICROSTEPS];
ocrb = microstepcurve[MICROSTEPS*2 - currentstep];
} else if ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) {
ocra = microstepcurve[MICROSTEPS*3 - currentstep];
ocrb = microstepcurve[currentstep - MICROSTEPS*2];
} else if ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) {
ocra = microstepcurve[currentstep - MICROSTEPS*3];
ocrb = microstepcurve[MICROSTEPS*4 - currentstep];
}
}
currentstep += MICROSTEPS*4;
currentstep %= MICROSTEPS*4;
// release all
uint8_t latch_state = 0; // all motor pins to 0
if (style == DW_MICROSTEP) {
if ((currentstep >= 0) && (currentstep < MICROSTEPS))
latch_state |= 0x03;
if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2))
latch_state |= 0x06;
if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3))
latch_state |= 0x0C;
if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4))
latch_state |= 0x09;
} else {
switch (currentstep/(MICROSTEPS/2)) {
case 0:
latch_state |= 0x1; // energize coil 1 only
break;
case 1:
latch_state |= 0x3; // energize coil 1+2
break;
case 2:
latch_state |= 0x2; // energize coil 2 only
break;
case 3:
latch_state |= 0x6; // energize coil 2+3
break;
case 4:
latch_state |= 0x4; // energize coil 3 only
break;
case 5:
latch_state |= 0xC; // energize coil 3+4
break;
case 6:
latch_state |= 0x8; // energize coil 4 only
break;
case 7:
latch_state |= 0x9; // energize coil 1+4
break;
}
}
if (latch_state & 0x1) {
DWC->setPin(AIN2pin, 1);
} else {
DWC->setPin(AIN2pin, 0);
}
if (latch_state & 0x2) {
DWC->setPin(BIN1pin, 1);
} else {
DWC->setPin(BIN1pin, 0);
}
if (latch_state & 0x4) {
DWC->setPin(AIN1pin, 1);
} else {
DWC->setPin(AIN1pin, 0);
}
if (latch_state & 0x8) {
DWC->setPin(BIN2pin, 1);
} else {
DWC->setPin(BIN2pin, 0);
}
return currentstep;
}

7
darkwater/DW640.h
View File

@@ -1,6 +1,6 @@
/*
Dark Water 640 driver code is placed under the BSD license.
Written by Team Dark Water (team@darkwater.io)
Written by Team Dark Water (team@darkwater.io) based off libraries by Adafruit https://github.com/adafruit/Adafruit_Motor_Shield_V2_Library
Copyright (c) 2014, Dark Water
All rights reserved.
@@ -150,11 +150,6 @@ class DW640 {
DW_Servo *getServo(uint8_t servo);
DW_Stepper *getStepper(uint16_t steps, uint8_t stepper);
// void setStepperOff(uint8_t stepper);
// void setStepperSpeed(uint8_t stepper, uint16_t speed);
// void oneStep(uint8_t stepper, uint8_t direction, uint8_t style);
// void step(uint8_t stepper, uint16_t steps, uint8_t direction, uint8_t style = DW_SINGLE);
private:
uint8_t devAddr;
float frequency;