main working code with go home, slide removal, and variable step size
This commit is contained in:
tp2043
542
src/firmware/SICKLESCOPE_NEW_CODE_CYTOCUBE2.ino
Normal file
542
src/firmware/SICKLESCOPE_NEW_CODE_CYTOCUBE2.ino
Normal file
@@ -0,0 +1,542 @@
|
||||
|
||||
#include <EEPROM.h>
|
||||
|
||||
#define Xdir 12
|
||||
#define Ydir 5
|
||||
#define Zdir 24
|
||||
|
||||
#define Xenable 7
|
||||
#define Yenable 15
|
||||
#define Zenable 34
|
||||
|
||||
#define Xbump A2
|
||||
#define Ybump A3
|
||||
#define Zbump A4
|
||||
|
||||
#define XFault 13
|
||||
#define YFault 6
|
||||
#define ZFault 40
|
||||
|
||||
/***************************************************************************************
|
||||
This is what matters when you change the Step size : change the FoV accordingly
|
||||
Rest of everything is calculated in terms of steps only
|
||||
So the GUI needs to take care of step size to um conversion
|
||||
***************************************************************************************/
|
||||
// FoV size. This is in steps. Calcuated as FoV (steps) = FoV (um) / stepSize
|
||||
// The step size as on 27 June is 198 nm / step
|
||||
//long xFoV = 1843, yFoV = 2323;
|
||||
|
||||
short T = 1; // Time Period/2 for PWM signal. Need to be made 2 when in Full Step mode
|
||||
|
||||
// updating for getting CDS data
|
||||
long xFoV = 1504, yFoV = 1200; //2000 1600//
|
||||
bool XmotorEnabled = false, YmotorEnabled = false, ZmotorEnabled = false;
|
||||
|
||||
long XMaxStep = 300000L, YMaxStep = 150000L, ZMaxStep = 200000L;
|
||||
long limitTOZfocus = 50000L, limitToXZero = 100000, limitToYZero = -20000; //limitToXZero=220000,limitToYZero=8000; Previous Values
|
||||
|
||||
long XcurrStep = 0L, YcurrStep = 0L, ZcurrStep = 0L;
|
||||
long Ax = 0L, Ay = 0L, Az = 0L, Bx = 0L, By = 0L, Bz = 0L, Cx = 0L, Cy = 0L, Cz = 0L;
|
||||
long nextZstep = 0, XdeltaZperFoV = 0, YdeltaZperFoV = 0;
|
||||
|
||||
void gotoXYZSetStep(long XsetStep = XcurrStep, long YsetStep = YcurrStep, long ZsetStep = ZcurrStep);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(57600);
|
||||
// pulse : X,Y,Z :: TX 1,2,3
|
||||
// Serial1.begin(38400); Serial2.begin(38400); Serial3.begin(38400);
|
||||
pinMode(Zdir, OUTPUT); pinMode(Ydir, OUTPUT); pinMode(Xdir, OUTPUT);
|
||||
//******************** The bump switchs****************************//
|
||||
pinMode(Xbump, INPUT); pinMode(Ybump, INPUT); pinMode(Zbump, INPUT);
|
||||
//******************* X,Y,Z -Axis M0,M1,M2 Pins******************//
|
||||
pinMode(8, OUTPUT); pinMode(9, OUTPUT); pinMode(10, OUTPUT); //X-Axis Step Size
|
||||
pinMode(14, OUTPUT); pinMode(2, OUTPUT); pinMode(3, OUTPUT); //Y-Axis Step Size
|
||||
pinMode(32, OUTPUT); pinMode(30, OUTPUT); pinMode(28, OUTPUT); //Z-axis Step Size
|
||||
//****************** X,Y-0.2um and Z-0.1um ********************//
|
||||
digitalWrite(8, LOW); digitalWrite(9, LOW); digitalWrite(10, HIGH);
|
||||
digitalWrite(14, LOW); digitalWrite(2, LOW); digitalWrite(3, HIGH);
|
||||
digitalWrite(32, HIGH); digitalWrite(30, HIGH); digitalWrite(28, HIGH);
|
||||
|
||||
pinMode(XFault, INPUT); pinMode(YFault, INPUT); pinMode(ZFault, INPUT);
|
||||
|
||||
pinMode(Xenable, OUTPUT); pinMode(Yenable, OUTPUT); pinMode(Zenable, OUTPUT);
|
||||
|
||||
digitalWrite(Xenable, HIGH); digitalWrite(Yenable, HIGH); digitalWrite(Zenable, HIGH);
|
||||
|
||||
//*************************************************************//
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
int choice = Serial.read();
|
||||
|
||||
switch (choice) {
|
||||
case ('T') : digitalWrite(Zdir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep - 1); // T == Z up by 5 counts
|
||||
break;
|
||||
|
||||
case ('G') : digitalWrite(Zdir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep + 1); // G == Z down
|
||||
break;
|
||||
|
||||
case ('t') : digitalWrite(Zdir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep - 100); // T == Z up by 5 counts
|
||||
break;
|
||||
|
||||
case ('g') : digitalWrite(Zdir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep + 100); // G == Z down
|
||||
break;
|
||||
|
||||
case ('C') : digitalWrite(Zdir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep - 5000); // T == Z up by 5 counts
|
||||
break;
|
||||
|
||||
case ('V') : digitalWrite(Zdir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep + 5000); // G == Z down
|
||||
break;
|
||||
|
||||
case ('c') : digitalWrite(Zdir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep - 1000); // T == Z up by 5 counts
|
||||
break;
|
||||
|
||||
case ('v') : digitalWrite(Zdir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep + 1000); // G == Z down
|
||||
break;
|
||||
|
||||
case ('A') : digitalWrite(Xdir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep - xFoV, YcurrStep, ZcurrStep);
|
||||
break;
|
||||
|
||||
case ('D') : digitalWrite(Xdir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep + xFoV, YcurrStep, ZcurrStep);
|
||||
break;
|
||||
|
||||
case ('W') : digitalWrite(Ydir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep + yFoV, ZcurrStep);
|
||||
break;
|
||||
|
||||
case ('S') : digitalWrite(Ydir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep - yFoV, ZcurrStep);
|
||||
break;
|
||||
|
||||
case ('J') : XcurrStep = 0; // J == X reset
|
||||
break;
|
||||
case ('K') : YcurrStep = 0; // K == Y reset
|
||||
break;
|
||||
case ('L') : ZcurrStep = 0; // L == Z reset
|
||||
break;
|
||||
case ('B') : gotoXYZSetStep(getStep(), YcurrStep, ZcurrStep);
|
||||
break;
|
||||
case ('N') : gotoXYZSetStep(XcurrStep, getStep(), ZcurrStep);
|
||||
break;
|
||||
case ('M') : gotoXYZSetStep(XcurrStep, YcurrStep, getStep());
|
||||
break;
|
||||
case (';') : gotoXYZSetStep(getStep(), getStep(), ZcurrStep);
|
||||
break;
|
||||
case ('[') : gotoXYZSetStep(getStep(), YcurrStep, getStep());
|
||||
break;
|
||||
case (']') : gotoXYZSetStep(XcurrStep, getStep(), getStep());
|
||||
break;
|
||||
case (',') : gotoXYZSetStep(getStep(), getStep(), getStep());
|
||||
break;
|
||||
break;
|
||||
case ('I') : Serial3.flush(); Serial.print('X'); Serial.print("#"); Serial.print(XcurrStep); Serial.print("#");
|
||||
break;
|
||||
case ('O') : Serial2.flush(); Serial.print('Y'); Serial.print("#"); Serial.print(YcurrStep); Serial.print("#");
|
||||
break;
|
||||
case ('P') : Serial1.flush(); Serial.print('Z'); Serial.print("#"); Serial.print(ZcurrStep); Serial.print("#");
|
||||
break;
|
||||
|
||||
// Set A,B,C - Codes are E,R,Y respectively
|
||||
case ('E') : Ax = XcurrStep; Ay = YcurrStep; Az = ZcurrStep;
|
||||
break;
|
||||
case ('R') : Bx = XcurrStep; By = YcurrStep; Bz = ZcurrStep;
|
||||
break;
|
||||
case ('Y') : Cx = XcurrStep; Cy = YcurrStep; Cz = ZcurrStep;
|
||||
break;
|
||||
case ('U') :
|
||||
break;
|
||||
case ('X') :
|
||||
break;
|
||||
// case ('V') : debug();
|
||||
// break;
|
||||
case ('Q') : gohome();
|
||||
break;
|
||||
case ('F') : slideIO();
|
||||
break;
|
||||
// case ('q') : changeMode();
|
||||
// break;
|
||||
// case ('f') : revertMode();
|
||||
// break;
|
||||
case ('1') :
|
||||
break;
|
||||
case ('2') :
|
||||
break;
|
||||
case ('0') : EEPROM.write(0, 0);
|
||||
break;
|
||||
case ('7') : Serial.print('7'); Serial.print("#");
|
||||
break;
|
||||
case ('3') : digitalWrite(Zdir, HIGH); delayMicroseconds(10); Serial3.print("X"); ZcurrStep = ZcurrStep + 1;
|
||||
break;
|
||||
case ('5') : digitalWrite(Zdir, LOW); delayMicroseconds(10); Serial3.print("X"); ZcurrStep = ZcurrStep - 1;
|
||||
|
||||
default : break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void gotoXYZSetStep(long XsetStep, long YsetStep, long ZsetStep)
|
||||
{
|
||||
|
||||
long Xstepstodo = 0, Ystepstodo = 0, Zstepstodo = 0;
|
||||
Serial.print("Current X, Y, Z = ");
|
||||
Serial.print(XcurrStep);
|
||||
Serial.print(", ");
|
||||
Serial.print(YcurrStep);
|
||||
Serial.print(", ");
|
||||
Serial.println(ZcurrStep);
|
||||
|
||||
if (XsetStep != XcurrStep) {
|
||||
// if (XsetStep > XMaxStep)
|
||||
// XsetStep = XMaxStep;
|
||||
|
||||
if (XsetStep > XcurrStep && (isXbumpHit() != 2))
|
||||
{
|
||||
Xstepstodo = abs(XsetStep - XcurrStep);
|
||||
digitalWrite(Xdir, LOW);
|
||||
XcurrStep = XsetStep;
|
||||
// doXYZSteps(Xstepstodo, Ystepstodo, Zstepstodo);
|
||||
}
|
||||
else if (XsetStep < XcurrStep && (isXbumpHit() != 1))
|
||||
{
|
||||
Xstepstodo = abs(XsetStep - XcurrStep);
|
||||
digitalWrite(Xdir, HIGH);
|
||||
XcurrStep = XsetStep;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (YsetStep != YcurrStep)
|
||||
{
|
||||
// if (YsetStep > YMaxStep)
|
||||
// YsetStep = YMaxStep;
|
||||
|
||||
if (YsetStep > YcurrStep && (isYbumpHit() != 2))
|
||||
{
|
||||
Ystepstodo = abs(YsetStep - YcurrStep);
|
||||
digitalWrite(Ydir, LOW);
|
||||
YcurrStep = YsetStep;
|
||||
}
|
||||
else if (YsetStep < YcurrStep && (isYbumpHit() != 1))
|
||||
{
|
||||
Ystepstodo = abs(YsetStep - YcurrStep);
|
||||
digitalWrite(Ydir, HIGH);
|
||||
YcurrStep = YsetStep;
|
||||
}
|
||||
}
|
||||
|
||||
if (ZsetStep != ZcurrStep) {
|
||||
// if (ZsetStep > ZMaxStep)
|
||||
// ZsetStep = ZMaxStep;
|
||||
|
||||
if (ZsetStep > ZcurrStep && (isZbumpHit() != 1))
|
||||
{
|
||||
Zstepstodo = abs(ZsetStep - ZcurrStep);
|
||||
digitalWrite(Zdir, HIGH);
|
||||
ZcurrStep = ZsetStep;
|
||||
}
|
||||
else if (ZsetStep < ZcurrStep && (isZbumpHit() != 2))
|
||||
{
|
||||
Zstepstodo = abs(ZsetStep - ZcurrStep);
|
||||
digitalWrite(Zdir, LOW);
|
||||
ZcurrStep = ZsetStep;
|
||||
}
|
||||
}
|
||||
doXYZSteps(Xstepstodo, Ystepstodo, Zstepstodo);
|
||||
// Serial.print("Fault Detected: X, Y, Z -> ");
|
||||
// Serial.print(digitalRead(XFault));
|
||||
// Serial.print(", ");
|
||||
// Serial.print(digitalRead(YFault));
|
||||
// Serial.print(", ");
|
||||
// Serial.println(digitalRead(ZFault));
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void doXYZSteps(long numX, long numY, long numZ)
|
||||
{
|
||||
// long XsetStep = numX; long YsetStep = numY; long ZsetStep = numZ;
|
||||
|
||||
while (numX > 0 || numY > 0 || numZ > 0)
|
||||
{
|
||||
if (XmotorEnabled)
|
||||
{
|
||||
if ( numX <= 0 )
|
||||
{
|
||||
// Serial.print("hit numX"); Serial.print("#");
|
||||
digitalWrite(Xenable, HIGH);
|
||||
XmotorEnabled = false;
|
||||
}
|
||||
|
||||
else if ( digitalRead (Xdir) == LOW && isXbumpHit() == 2)
|
||||
{
|
||||
// Serial.print("hit low"); Serial.print("#");
|
||||
XcurrStep = XcurrStep - numX;
|
||||
// Serial.println("X2");
|
||||
numX = 0;
|
||||
}
|
||||
else if (digitalRead (Xdir) == HIGH && isXbumpHit() == 1)
|
||||
{
|
||||
// Serial.print("hit high"); Serial.print("#");
|
||||
XcurrStep = XcurrStep + numX;
|
||||
// Serial.println("X1");
|
||||
|
||||
numX = 0;
|
||||
}
|
||||
}
|
||||
else if (numX > 0)
|
||||
{
|
||||
digitalWrite(Xenable, LOW);
|
||||
XmotorEnabled = true;
|
||||
}
|
||||
|
||||
|
||||
if (YmotorEnabled)
|
||||
{
|
||||
if ( numY <= 0 )
|
||||
{
|
||||
digitalWrite(Yenable, HIGH);
|
||||
YmotorEnabled = false;
|
||||
}
|
||||
|
||||
else if (digitalRead (Ydir) == LOW && isYbumpHit() == 2)
|
||||
{
|
||||
YcurrStep = YcurrStep - numY;
|
||||
// Serial.println("Y2");
|
||||
numY = 0;
|
||||
}
|
||||
else if (digitalRead (Ydir) == HIGH && isYbumpHit() == 1)
|
||||
{
|
||||
YcurrStep = YcurrStep + numY;
|
||||
// Serial.println("Y1");
|
||||
|
||||
numY = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (numY > 0)
|
||||
{
|
||||
digitalWrite(Yenable, LOW);
|
||||
YmotorEnabled = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (ZmotorEnabled)
|
||||
{
|
||||
if ( numZ <= 200 ) // Avoiding Collision with slide. Can be modified later
|
||||
{
|
||||
digitalWrite(Zenable, HIGH);
|
||||
ZmotorEnabled = false;
|
||||
}
|
||||
else if (digitalRead (Zdir) == LOW && isZbumpHit() == 2)
|
||||
{
|
||||
ZcurrStep = ZcurrStep + numZ;
|
||||
// Serial.println("Z2");
|
||||
numZ = 0;
|
||||
}
|
||||
else if (digitalRead (Zdir) == HIGH && isZbumpHit() == 1)
|
||||
{
|
||||
ZcurrStep = ZcurrStep - numZ;
|
||||
// Serial.println("Z1");
|
||||
numZ = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (numZ > 0)
|
||||
{
|
||||
digitalWrite(Zenable, LOW);
|
||||
ZmotorEnabled = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// PWM for motor driver
|
||||
digitalWrite(11, LOW); digitalWrite(4, LOW); digitalWrite(26, LOW);
|
||||
delayMicroseconds(T);
|
||||
digitalWrite(11, HIGH); digitalWrite(4, HIGH); digitalWrite(26, HIGH);
|
||||
delayMicroseconds(T);
|
||||
// digitalWrite(11, LOW); digitalWrite(4, LOW); digitalWrite(26, LOW);
|
||||
|
||||
if (XmotorEnabled)
|
||||
{
|
||||
numX--; // Delay added for motor motion syncing with signal
|
||||
if (numX <= 5) delay(1);
|
||||
}
|
||||
if (YmotorEnabled)
|
||||
{
|
||||
numY--; // and ramping out at the end
|
||||
if (numY <= 5) delay(1);
|
||||
}
|
||||
if (ZmotorEnabled)
|
||||
{
|
||||
numZ--;
|
||||
if (numZ <= 5) delay(1);
|
||||
}
|
||||
|
||||
}
|
||||
Serial.print("New X, Y, Z = ");
|
||||
Serial.print(XcurrStep);
|
||||
Serial.print(", ");
|
||||
Serial.print(YcurrStep);
|
||||
Serial.print(", ");
|
||||
Serial.println(ZcurrStep);
|
||||
}
|
||||
|
||||
int isXbumpHit()
|
||||
{
|
||||
int xval = analogRead(Xbump);
|
||||
|
||||
if (xval <= 200) {
|
||||
return 1 ;
|
||||
}
|
||||
if ((xval > 200) && ((xval <= 800))) {
|
||||
return 2;
|
||||
}
|
||||
if (xval > 800) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int isYbumpHit()
|
||||
{
|
||||
int yval = analogRead(Ybump);
|
||||
|
||||
if (yval <= 200) {
|
||||
return 1 ;
|
||||
}
|
||||
if ((yval > 200) && ((yval <= 800))) {
|
||||
return 2;
|
||||
}
|
||||
if (yval > 800) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int isZbumpHit()
|
||||
{
|
||||
int zval = analogRead(Zbump);
|
||||
|
||||
if (zval <= 200) {
|
||||
|
||||
return 1 ;
|
||||
}
|
||||
if ((zval > 200) && ((zval <= 800))) {
|
||||
|
||||
return 2;
|
||||
}
|
||||
if (zval > 800) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
long getStep()
|
||||
{
|
||||
long index = 0, inpStep[7] = {0, 0, 0, 0, 0, 0, 0}, invalidInput = 0;
|
||||
long setStep = 0L;
|
||||
|
||||
|
||||
for (index = 0; index < 8; index++ )
|
||||
{
|
||||
while ( Serial.available() < 1 );
|
||||
inpStep[index] = Serial.read() - 48;
|
||||
}
|
||||
|
||||
// DISCARD first byte : MATLAB sends the terminator character there. We need it since if we turn it off, MATALB won't read the Tx from this arduino
|
||||
// ALSO igmore the second byte : that's the sign symbol
|
||||
setStep = (inpStep[2] * 100000) + (inpStep[3] * 10000) + (inpStep[4] * 1000) + (inpStep[5] * 100) + (inpStep[6] * 10) + (inpStep[7] * 1) + 0L ;
|
||||
|
||||
// check what sign was sent. If it was anything other than a '-' then dont worry
|
||||
if (inpStep[1] + 48 == '-' ) {
|
||||
setStep = setStep * (-1);
|
||||
}
|
||||
return setStep;
|
||||
}
|
||||
|
||||
|
||||
void enableMotors() {
|
||||
digitalWrite(Xenable, LOW); digitalWrite(Yenable, LOW); digitalWrite(Zenable, LOW);
|
||||
}
|
||||
void disableMotors() {
|
||||
digitalWrite(Xenable, HIGH); digitalWrite(Yenable, HIGH); digitalWrite(Zenable, HIGH);
|
||||
}
|
||||
|
||||
|
||||
void changeMode(){
|
||||
digitalWrite(8, LOW); digitalWrite(9, HIGH); digitalWrite(10, LOW); // Probably due to voltage issues it won't go for steps larger than 1/8. So 1/8, 1/16, 1/32 all work.
|
||||
digitalWrite(14, HIGH); digitalWrite(2, LOW); digitalWrite(3, LOW);
|
||||
digitalWrite(32, HIGH); digitalWrite(30, LOW); digitalWrite(28, LOW);
|
||||
T = 2;
|
||||
}
|
||||
|
||||
void revertMode(){
|
||||
digitalWrite(8, LOW); digitalWrite(9, LOW); digitalWrite(10, HIGH);
|
||||
digitalWrite(14, LOW); digitalWrite(2, LOW); digitalWrite(3, HIGH);
|
||||
digitalWrite(32, HIGH); digitalWrite(30, HIGH); digitalWrite(28, HIGH);
|
||||
T = 1;
|
||||
}
|
||||
|
||||
|
||||
//88888888888888888888888888888888888888888888888888888888888888888
|
||||
// THIS IS THE GOHOME() FUNCTION
|
||||
//88888888888888888888888888888888888888888888888888888888888888888
|
||||
void gohome()
|
||||
{
|
||||
changeMode();
|
||||
gotoXYZSetStep(500000, -200000, -200000);
|
||||
if((isXbumpHit()==2) && (isYbumpHit()==1) && (isZbumpHit()==2))
|
||||
{
|
||||
// delay(1000);
|
||||
revertMode();
|
||||
return;
|
||||
// XcurrStep = 0L; YcurrStep = 0L; ZcurrStep = 0L;
|
||||
}
|
||||
// enableMotors();
|
||||
// gotoXSetStep(-100000);
|
||||
// disableMotors();
|
||||
// Serial.print("S#");
|
||||
}
|
||||
void slideIO() // to make the insert/remove the slide
|
||||
{
|
||||
changeMode();
|
||||
gotoXYZSetStep(-500000, 200000, 200000);
|
||||
if((isXbumpHit()==1) && (isYbumpHit()==2) && (isZbumpHit()==1))
|
||||
{
|
||||
// delay(1000);
|
||||
revertMode();
|
||||
return;
|
||||
// XcurrStep = 0L; YcurrStep = 0L; ZcurrStep = 0L;
|
||||
}
|
||||
// enableMotors();
|
||||
// gotoXSetStep(-100000);
|
||||
// disableMotors();
|
||||
// Serial.print("S#");
|
||||
}
|
||||
void debug()
|
||||
{
|
||||
|
||||
// calc steps in X
|
||||
long XstepCount = abs(Bx - Ax) ; long numXsteps = (XstepCount / xFoV) ;
|
||||
|
||||
// calc steps in Y
|
||||
long YstepCount = abs(Cy - By) ; long numYsteps = (YstepCount / yFoV) ;
|
||||
|
||||
// calc the z changes
|
||||
long XZdeltaCount = Bz - Az ; long XdeltaZperFoV = XZdeltaCount / (numXsteps - 1);
|
||||
long YZdeltaCount = Cz - Bz ; long YdeltaZperFoV = YZdeltaCount / (numYsteps - 1);
|
||||
|
||||
// set the initial delta z to the x one. Later, in the loop, it will be changed continously
|
||||
long deltaZperFoV = XdeltaZperFoV;
|
||||
|
||||
Serial.print(" Ax: "); Serial.print(Ax); Serial.print(" Ay: "); Serial.print(Ay); Serial.print(" Az: "); Serial.print(Az);
|
||||
Serial.print(" Bx: "); Serial.print(Bx); Serial.print(" By: "); Serial.print(By); Serial.print(" Bz: "); Serial.print(Bz);
|
||||
Serial.print(" Cx: "); Serial.print(Cx); Serial.print(" Cy: "); Serial.print(Cy); Serial.print(" Cz: "); Serial.print(Cz);
|
||||
Serial.print(" XdeltaZperFoV: "); Serial.print(XdeltaZperFoV);
|
||||
Serial.print(" YdeltaZperFoV: "); Serial.print(YdeltaZperFoV);
|
||||
Serial.print("#");
|
||||
|
||||
}
|
||||
590
src/firmware/SICKLESCOPE_NEW_CODE_CYTOCUBE3.ino
Normal file
590
src/firmware/SICKLESCOPE_NEW_CODE_CYTOCUBE3.ino
Normal file
@@ -0,0 +1,590 @@
|
||||
/**************************************************************************************
|
||||
**************************************************************************************
|
||||
* Date: 27/03/2023
|
||||
* File Name: CytoCube v1.3
|
||||
* Description: All glitches fixed
|
||||
**************************************************************************************
|
||||
*************************************************************************************/
|
||||
#include <EEPROM.h>
|
||||
|
||||
#define Xdir 12
|
||||
#define Ydir 5
|
||||
#define Zdir 24
|
||||
|
||||
#define Xenable 7
|
||||
#define Yenable 15
|
||||
#define Zenable 34
|
||||
|
||||
#define Xbump A2
|
||||
#define Ybump A3
|
||||
#define Zbump A4
|
||||
//
|
||||
//#define XFault 13
|
||||
//#define YFault 6
|
||||
//#define ZFault 40
|
||||
|
||||
/***************************************************************************************
|
||||
This is what matters when you change the Step size : change the FoV accordingly
|
||||
Rest of everything is calculated in terms of steps only
|
||||
So the GUI needs to take care of step size to um conversion
|
||||
***************************************************************************************/
|
||||
// FoV size. This is in steps. Calcuated as FoV (steps) = FoV (um) / stepSize
|
||||
// The step size as on 27 June is 198 nm / step
|
||||
//long xFoV = 1843, yFoV = 2323;
|
||||
|
||||
//short T = 1; // Time Period/2 for PWM signal. Need to be made 2 when in Full Step mode
|
||||
|
||||
// updating for getting CDS data
|
||||
long xFoV = 1504L, yFoV = 1200L; //2000 1600//
|
||||
bool XmotorEnabled = false, YmotorEnabled = false, ZmotorEnabled = false;
|
||||
|
||||
long XMaxStep = 300000L, YMaxStep = 150000L, ZMaxStep = 200000L;
|
||||
long limitTOZfocus = 50000L, limitToXZero = 100000, limitToYZero = -20000; //limitToXZero=220000,limitToYZero=8000; Previous Values
|
||||
|
||||
signed long XcurrStep = 0L, YcurrStep = 0L, ZcurrStep = 0L;
|
||||
long Ax = 0L, Ay = 0L, Az = 0L, Bx = 0L, By = 0L, Bz = 0L, Cx = 0L, Cy = 0L, Cz = 0L;
|
||||
long nextZstep = 0, XdeltaZperFoV = 0, YdeltaZperFoV = 0;
|
||||
|
||||
int lastCommand;
|
||||
|
||||
void gotoXYZSetStep(long XsetStep = XcurrStep, long YsetStep = YcurrStep, long ZsetStep = ZcurrStep);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(57600);
|
||||
// pulse : X,Y,Z :: TX 1,2,3
|
||||
// Serial1.begin(38400); Serial2.begin(38400); Serial3.begin(38400);
|
||||
pinMode(Zdir, OUTPUT); pinMode(Ydir, OUTPUT); pinMode(Xdir, OUTPUT);
|
||||
//******************** The bump switchs****************************//
|
||||
pinMode(Xbump, INPUT); pinMode(Ybump, INPUT); pinMode(Zbump, INPUT);
|
||||
//******************* X,Y,Z -Axis M0,M1,M2 Pins******************//
|
||||
pinMode(8, OUTPUT); pinMode(9, OUTPUT); pinMode(10, OUTPUT); //X-Axis Step Size
|
||||
pinMode(14, OUTPUT); pinMode(2, OUTPUT); pinMode(3, OUTPUT); //Y-Axis Step Size
|
||||
pinMode(32, OUTPUT); pinMode(30, OUTPUT); pinMode(28, OUTPUT); //Z-axis Step Size
|
||||
//****************** X,Y-0.2um and Z-0.1um ********************//
|
||||
digitalWrite(8, LOW); digitalWrite(9, LOW); digitalWrite(10, HIGH);
|
||||
digitalWrite(14, LOW); digitalWrite(2, LOW); digitalWrite(3, HIGH);
|
||||
digitalWrite(32, HIGH); digitalWrite(30, HIGH); digitalWrite(28, HIGH);
|
||||
|
||||
//pinMode(XFault, INPUT); pinMode(YFault, INPUT); pinMode(ZFault, INPUT);
|
||||
|
||||
pinMode(Xenable, OUTPUT); pinMode(Yenable, OUTPUT); pinMode(Zenable, OUTPUT);
|
||||
|
||||
digitalWrite(Xenable, HIGH); digitalWrite(Yenable, HIGH); digitalWrite(Zenable, HIGH);
|
||||
|
||||
//*************************************************************//
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if(Serial.available()){
|
||||
int choice = Serial.read();
|
||||
|
||||
switch (choice) {
|
||||
case ('T') : digitalWrite(Zdir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep - 1); // T == Z up by 5 counts
|
||||
break;
|
||||
|
||||
case ('G') : digitalWrite(Zdir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep + 1); // G == Z down
|
||||
break;
|
||||
|
||||
case ('t') : digitalWrite(Zdir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep - 100); // T == Z up by 5 counts
|
||||
break;
|
||||
|
||||
case ('g') : digitalWrite(Zdir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep + 100); // G == Z down
|
||||
break;
|
||||
|
||||
case ('C') : digitalWrite(Zdir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep - 5000); // T == Z up by 5 counts
|
||||
break;
|
||||
|
||||
case ('V') : digitalWrite(Zdir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep + 5000); // G == Z down
|
||||
break;
|
||||
|
||||
case ('c') : digitalWrite(Zdir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep - 1000); // T == Z up by 5 counts
|
||||
break;
|
||||
|
||||
case ('v') : digitalWrite(Zdir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep, ZcurrStep + 1000); // G == Z down
|
||||
break;
|
||||
|
||||
case ('A') : digitalWrite(Xdir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep - xFoV, YcurrStep, ZcurrStep);
|
||||
break;
|
||||
|
||||
case ('D') : digitalWrite(Xdir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep + xFoV, YcurrStep, ZcurrStep);
|
||||
break;
|
||||
|
||||
case ('W') : digitalWrite(Ydir, LOW); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep + yFoV, ZcurrStep);
|
||||
break;
|
||||
|
||||
case ('S') : digitalWrite(Ydir, HIGH); delayMicroseconds(10); gotoXYZSetStep(XcurrStep, YcurrStep - yFoV, ZcurrStep);
|
||||
break;
|
||||
|
||||
case ('J') : XcurrStep = 0L; // J == X reset
|
||||
break;
|
||||
case ('K') : YcurrStep = 0L; // K == Y reset
|
||||
break;
|
||||
case ('L') : ZcurrStep = 0L; // L == Z reset
|
||||
break;
|
||||
case ('B') : gotoXYZSetStep(getStep(), YcurrStep, ZcurrStep);
|
||||
break;
|
||||
case ('N') : gotoXYZSetStep(XcurrStep, getStep(), ZcurrStep);
|
||||
break;
|
||||
case ('M') : gotoXYZSetStep(XcurrStep, YcurrStep, getStep());
|
||||
break;
|
||||
case (';') : gotoXYZSetStep(getStep(), getStep(), ZcurrStep);
|
||||
break;
|
||||
case ('[') : gotoXYZSetStep(getStep(), YcurrStep, getStep());
|
||||
break;
|
||||
case (']') : gotoXYZSetStep(XcurrStep, getStep(), getStep());
|
||||
break;
|
||||
case (',') : gotoXYZSetStep(getStep(), getStep(), getStep());
|
||||
break;
|
||||
// break;
|
||||
case ('I') : Serial3.flush(); Serial.print('X'); Serial.print("#"); Serial.print(XcurrStep); Serial.print("#");
|
||||
break;
|
||||
case ('O') : Serial2.flush(); Serial.print('Y'); Serial.print("#"); Serial.print(YcurrStep); Serial.print("#");
|
||||
break;
|
||||
case ('P') : Serial1.flush(); Serial.print('Z'); Serial.print("#"); Serial.print(ZcurrStep); Serial.print("#");
|
||||
break;
|
||||
|
||||
// Set A,B,C - Codes are E,R,Y respectively
|
||||
case ('E') : Ax = XcurrStep; Ay = YcurrStep; Az = ZcurrStep;
|
||||
break;
|
||||
case ('R') : Bx = XcurrStep; By = YcurrStep; Bz = ZcurrStep;
|
||||
break;
|
||||
case ('Y') : Cx = XcurrStep; Cy = YcurrStep; Cz = ZcurrStep;
|
||||
break;
|
||||
case ('U') :
|
||||
break;
|
||||
case ('X') : XcurrStep = 0L; YcurrStep = 0L; ZcurrStep = 0L;
|
||||
break;
|
||||
// case ('V') : debug();
|
||||
// break;
|
||||
case ('Q') : (lastCommand != 81) ? gohome(), revertMode():doNothing();
|
||||
break;
|
||||
case ('F') : (lastCommand != 70) ? slideIO(), revertMode():doNothing();
|
||||
break;
|
||||
case ('q') : changeMode();
|
||||
break;
|
||||
case ('f') : revertMode();
|
||||
break;
|
||||
case ('1') :
|
||||
break;
|
||||
case ('2') :
|
||||
break;
|
||||
case ('0') : EEPROM.write(0, 0);
|
||||
break;
|
||||
case ('7') : Serial.print('7'); Serial.print("#");
|
||||
break;
|
||||
case ('3') : digitalWrite(Zdir, HIGH); delayMicroseconds(10); Serial3.print("X"); ZcurrStep = ZcurrStep + 1;
|
||||
break;
|
||||
case ('5') : digitalWrite(Zdir, LOW); delayMicroseconds(10); Serial3.print("X"); ZcurrStep = ZcurrStep - 1;
|
||||
|
||||
default : break;
|
||||
}
|
||||
if(choice!=10){lastCommand = choice;}
|
||||
// Serial.print("last cmd = ");
|
||||
// Serial.println(lastCommand);
|
||||
}
|
||||
}
|
||||
|
||||
void doNothing(){
|
||||
Serial.println(":)");}
|
||||
|
||||
void gotoXYZSetStep(long XsetStep, long YsetStep, long ZsetStep)
|
||||
{
|
||||
|
||||
long Xstepstodo = 0L, Ystepstodo = 0L, Zstepstodo = 0L;
|
||||
Serial.print("\nCurrent X, Y, Z = ");
|
||||
Serial.print(XcurrStep);
|
||||
Serial.print(", ");
|
||||
Serial.print(YcurrStep);
|
||||
Serial.print(", ");
|
||||
Serial.println(ZcurrStep);
|
||||
Serial.print("Setting X, Y, Z = ");
|
||||
Serial.print(XsetStep);
|
||||
Serial.print(", ");
|
||||
Serial.print(YsetStep);
|
||||
Serial.print(", ");
|
||||
Serial.println(ZsetStep);
|
||||
if (ZsetStep != ZcurrStep) {
|
||||
// if (ZsetStep > ZMaxStep)
|
||||
// ZsetStep = ZMaxStep;
|
||||
|
||||
if (ZsetStep > ZcurrStep && (isZbumpHit() != 1))
|
||||
{
|
||||
Zstepstodo = abs(ZsetStep - ZcurrStep);
|
||||
// Serial.print("Z1steps to do: ");
|
||||
// Serial.println(Zstepstodo);
|
||||
digitalWrite(Zdir, HIGH);
|
||||
ZcurrStep = ZsetStep;
|
||||
}
|
||||
else if (ZsetStep < ZcurrStep && (isZbumpHit() != 2))
|
||||
{
|
||||
Zstepstodo = abs(ZsetStep - ZcurrStep);
|
||||
// Serial.print("Z2steps to do: ");
|
||||
// Serial.println(Zstepstodo);
|
||||
digitalWrite(Zdir, LOW);
|
||||
ZcurrStep = ZsetStep;
|
||||
}
|
||||
}
|
||||
if (XsetStep != XcurrStep) {
|
||||
// if (XsetStep > XMaxStep)
|
||||
// XsetStep = XMaxStep;
|
||||
|
||||
if (XsetStep > XcurrStep && (isXbumpHit() != 2))
|
||||
{
|
||||
Xstepstodo = abs(XsetStep - XcurrStep);
|
||||
digitalWrite(Xdir, LOW);
|
||||
XcurrStep = XsetStep;
|
||||
// doXYZSteps(Xstepstodo, Ystepstodo, Zstepstodo);
|
||||
}
|
||||
else if (XsetStep < XcurrStep && (isXbumpHit() != 1))
|
||||
{
|
||||
Xstepstodo = abs(XsetStep - XcurrStep);
|
||||
digitalWrite(Xdir, HIGH);
|
||||
XcurrStep = XsetStep;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (YsetStep != YcurrStep)
|
||||
{
|
||||
// if (YsetStep > YMaxStep)
|
||||
// YsetStep = YMaxStep;
|
||||
|
||||
if (YsetStep > YcurrStep && (isYbumpHit() != 2))
|
||||
{
|
||||
Ystepstodo = abs(YsetStep - YcurrStep);
|
||||
digitalWrite(Ydir, LOW);
|
||||
YcurrStep = YsetStep;
|
||||
}
|
||||
else if (YsetStep < YcurrStep && (isYbumpHit() != 1))
|
||||
{
|
||||
Ystepstodo = abs(YsetStep - YcurrStep);
|
||||
digitalWrite(Ydir, HIGH);
|
||||
YcurrStep = YsetStep;
|
||||
}
|
||||
}
|
||||
|
||||
doXYZSteps(Xstepstodo, Ystepstodo, Zstepstodo);
|
||||
// Serial.print("Fault Detected: X, Y, Z -> ");
|
||||
// Serial.print(digitalRead(XFault));
|
||||
// Serial.print(", ");
|
||||
// Serial.print(digitalRead(YFault));
|
||||
// Serial.print(", ");
|
||||
// Serial.println(digitalRead(ZFault));
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void doXYZSteps(long numX, long numY, long numZ)
|
||||
{
|
||||
// long XsetStep = numX; long YsetStep = numY; long ZsetStep = numZ;
|
||||
// Serial.print("NumX, NumY, NumZ: ");
|
||||
// Serial.print(numX);
|
||||
// Serial.print(", ");
|
||||
// Serial.print(numY);
|
||||
// Serial.print(", ");
|
||||
// Serial.println(numZ);
|
||||
while (numX > 0 || numY > 0 || numZ > 0)
|
||||
{
|
||||
if (XmotorEnabled)
|
||||
{
|
||||
if ( numX <= 0 )
|
||||
{
|
||||
// Serial.print("hit numX"); Serial.print("#");
|
||||
digitalWrite(Xenable, HIGH);
|
||||
XmotorEnabled = false;
|
||||
}
|
||||
|
||||
else if ( digitalRead (Xdir) == LOW && isXbumpHit() == 2)
|
||||
{
|
||||
// Serial.print("hit low"); Serial.print("#");
|
||||
XcurrStep = XcurrStep - numX;
|
||||
// Serial.println("X2");
|
||||
numX = 0;
|
||||
}
|
||||
else if (digitalRead (Xdir) == HIGH && isXbumpHit() == 1)
|
||||
{
|
||||
// Serial.print("hit high"); Serial.print("#");
|
||||
XcurrStep = XcurrStep + numX;
|
||||
// Serial.println("X1");
|
||||
|
||||
numX = 0;
|
||||
}
|
||||
}
|
||||
else if (numX > 0)
|
||||
{
|
||||
digitalWrite(Xenable, LOW);
|
||||
XmotorEnabled = true;
|
||||
}
|
||||
|
||||
|
||||
if (YmotorEnabled)
|
||||
{
|
||||
if ( numY <= 0 )
|
||||
{
|
||||
digitalWrite(Yenable, HIGH);
|
||||
YmotorEnabled = false;
|
||||
}
|
||||
|
||||
else if (digitalRead (Ydir) == LOW && isYbumpHit() == 2)
|
||||
{
|
||||
YcurrStep = YcurrStep - numY;
|
||||
// Serial.println("Y2");
|
||||
numY = 0;
|
||||
}
|
||||
else if (digitalRead (Ydir) == HIGH && isYbumpHit() == 1)
|
||||
{
|
||||
YcurrStep = YcurrStep + numY;
|
||||
// Serial.println("Y1");
|
||||
|
||||
numY = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (numY > 0)
|
||||
{
|
||||
digitalWrite(Yenable, LOW);
|
||||
YmotorEnabled = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (ZmotorEnabled)
|
||||
{
|
||||
if ( numZ <= 0 ) // Avoiding Collision with slide. Can be modified later
|
||||
{
|
||||
digitalWrite(Zenable, HIGH);
|
||||
ZmotorEnabled = false;
|
||||
}
|
||||
else if (digitalRead (Zdir) == LOW && isZbumpHit() == 2)
|
||||
{
|
||||
ZcurrStep = ZcurrStep + numZ;
|
||||
// Serial.println("Z2");
|
||||
numZ = 0;
|
||||
}
|
||||
else if (digitalRead (Zdir) == HIGH && isZbumpHit() == 1)
|
||||
{
|
||||
ZcurrStep = ZcurrStep - numZ;
|
||||
// Serial.println("Z1");
|
||||
numZ = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (numZ > 0)
|
||||
{
|
||||
digitalWrite(Zenable, LOW);
|
||||
ZmotorEnabled = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// PWM for motor driver
|
||||
digitalWrite(11, LOW); digitalWrite(4, LOW); digitalWrite(26, LOW);
|
||||
delayMicroseconds(1);
|
||||
digitalWrite(11, HIGH); digitalWrite(4, HIGH); digitalWrite(26, HIGH);
|
||||
delayMicroseconds(1);
|
||||
// digitalWrite(11, LOW); digitalWrite(4, LOW); digitalWrite(26, LOW);
|
||||
|
||||
if (XmotorEnabled)
|
||||
{
|
||||
numX--; // Delay added for motor motion syncing with signal
|
||||
if (numX <= 5) delay(1);
|
||||
}
|
||||
if (YmotorEnabled)
|
||||
{
|
||||
numY--; // and ramping out at the end
|
||||
if (numY <= 5) delay(1);
|
||||
}
|
||||
if (ZmotorEnabled)
|
||||
{
|
||||
numZ--;
|
||||
if (numZ <= 5) delay(1);
|
||||
}
|
||||
|
||||
}
|
||||
Serial.print("New X, Y, Z = ");
|
||||
Serial.print(XcurrStep);
|
||||
Serial.print(", ");
|
||||
Serial.print(YcurrStep);
|
||||
Serial.print(", ");
|
||||
Serial.println(ZcurrStep);
|
||||
}
|
||||
|
||||
int isXbumpHit()
|
||||
{
|
||||
int xval = analogRead(Xbump);
|
||||
|
||||
if (xval <= 200) {
|
||||
// Serial.println("X1");
|
||||
return 1 ;
|
||||
}
|
||||
if ((xval > 200) && ((xval <= 800))) {
|
||||
// Serial.println("X2");
|
||||
return 2;
|
||||
}
|
||||
if (xval > 800) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int isYbumpHit()
|
||||
{
|
||||
int yval = analogRead(Ybump);
|
||||
|
||||
if (yval <= 200) {
|
||||
// Serial.println("Y1");
|
||||
return 1 ;
|
||||
}
|
||||
if ((yval > 200) && ((yval <= 800))) {
|
||||
// Serial.println("Y2");
|
||||
return 2;
|
||||
}
|
||||
if (yval > 800) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int isZbumpHit()
|
||||
{
|
||||
int zval = analogRead(Zbump);
|
||||
|
||||
if (zval <= 200) {
|
||||
// Serial.println("Z1");
|
||||
return 1 ;
|
||||
}
|
||||
if ((zval > 200) && ((zval <= 800))) {
|
||||
// Serial.println("Z2");
|
||||
return 2;
|
||||
}
|
||||
if (zval > 800) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
long getStep()
|
||||
{
|
||||
long index = 0, inpStep[7] = {0, 0, 0, 0, 0, 0, 0}, invalidInput = 0;
|
||||
long setStep = 0L;
|
||||
|
||||
|
||||
for (index = 0; index < 8; index++ )
|
||||
{
|
||||
while ( Serial.available() < 1 );
|
||||
inpStep[index] = Serial.read() - 48;
|
||||
}
|
||||
|
||||
// DISCARD first byte : MATLAB sends the terminator character there. We need it since if we turn it off, MATALB won't read the Tx from this arduino
|
||||
// ALSO igmore the second byte : that's the sign symbol
|
||||
setStep = (inpStep[2] * 100000L) + (inpStep[3] * 10000L) + (inpStep[4] * 1000L) + (inpStep[5] * 100L) + (inpStep[6] * 10L) + (inpStep[7] * 1L) + 0L ;
|
||||
|
||||
// check what sign was sent. If it was anything other than a '-' then dont worry
|
||||
if (inpStep[1] + 48 == '-' ) {
|
||||
setStep = setStep * (-1L);
|
||||
}
|
||||
return setStep;
|
||||
}
|
||||
|
||||
|
||||
void enableMotors() {
|
||||
digitalWrite(Xenable, LOW); digitalWrite(Yenable, LOW); digitalWrite(Zenable, LOW);
|
||||
}
|
||||
void disableMotors() {
|
||||
digitalWrite(Xenable, HIGH); digitalWrite(Yenable, HIGH); digitalWrite(Zenable, HIGH);
|
||||
}
|
||||
|
||||
|
||||
void changeMode(){
|
||||
// bool xstate, ystate, zstate;
|
||||
//
|
||||
// xstate = digitalRead(Xenable);
|
||||
// ystate = digitalRead(Yenable);
|
||||
// zstate = digitalRead(Zenable);
|
||||
//
|
||||
// digitalWrite(Xenable, HIGH); digitalWrite(Yenable, HIGH); digitalWrite(Zenable, HIGH);
|
||||
digitalWrite(8, LOW); digitalWrite(9, HIGH); digitalWrite(10, LOW); // Probably due to voltage issues x axis motor won't go for steps larger than 1/4. So 1/4, 1/8, 1/16, 1/32 all work.
|
||||
digitalWrite(14, LOW); digitalWrite(2, HIGH); digitalWrite(3, LOW); // Working with 1/4 step mode for noise removal
|
||||
digitalWrite(32, LOW); digitalWrite(30, HIGH); digitalWrite(28, LOW);
|
||||
// digitalWrite(Xenable, xstate); digitalWrite(Yenable, ystate); digitalWrite(Zenable, zstate);
|
||||
|
||||
//T = ;
|
||||
}
|
||||
|
||||
void revertMode(){
|
||||
// bool xstate, ystate, zstate;
|
||||
//
|
||||
// xstate = digitalRead(Xenable);
|
||||
// ystate = digitalRead(Yenable);
|
||||
// zstate = digitalRead(Zenable);
|
||||
// digitalWrite(Xenable, HIGH); digitalWrite(Yenable, HIGH); digitalWrite(Zenable, HIGH);
|
||||
digitalWrite(8, LOW); digitalWrite(9, LOW); digitalWrite(10, HIGH);
|
||||
digitalWrite(14, LOW); digitalWrite(2, LOW); digitalWrite(3, HIGH);
|
||||
digitalWrite(32, HIGH); digitalWrite(30, HIGH); digitalWrite(28, HIGH);
|
||||
// digitalWrite(Xenable, xstate); digitalWrite(Yenable, ystate); digitalWrite(Zenable, zstate);
|
||||
//T = 1;
|
||||
}
|
||||
|
||||
|
||||
//88888888888888888888888888888888888888888888888888888888888888888
|
||||
// THIS IS THE GOHOME() FUNCTION
|
||||
//88888888888888888888888888888888888888888888888888888888888888888
|
||||
void gohome()
|
||||
{
|
||||
changeMode();
|
||||
gotoXYZSetStep(500000, -200000, -200000);
|
||||
if((isXbumpHit()==2) && (isYbumpHit()==1) && (isZbumpHit()==2))
|
||||
{
|
||||
// delay(1000);
|
||||
// revertMode();
|
||||
XcurrStep = 0L; YcurrStep = 0L; ZcurrStep = 0L;
|
||||
return;
|
||||
// XcurrStep = 0L; YcurrStep = 0L; ZcurrStep = 0L;
|
||||
}
|
||||
// enableMotors();
|
||||
// gotoXSetStep(-100000);
|
||||
// disableMotors();
|
||||
// Serial.print("S#");
|
||||
}
|
||||
void slideIO() // to make the insert/remove the slide
|
||||
{
|
||||
changeMode();
|
||||
gotoXYZSetStep(-500000, 200000, 200000);
|
||||
if((isXbumpHit()==1) && (isYbumpHit()==2) && (isZbumpHit()==1))
|
||||
{
|
||||
// delay(1000);
|
||||
// revertMode();
|
||||
return;
|
||||
// XcurrStep = 0L; YcurrStep = 0L; ZcurrStep = 0L;
|
||||
}
|
||||
// enableMotors();
|
||||
// gotoXSetStep(-100000);
|
||||
// disableMotors();
|
||||
// Serial.print("S#");
|
||||
}
|
||||
void debug()
|
||||
{
|
||||
|
||||
// calc steps in X
|
||||
long XstepCount = abs(Bx - Ax) ; long numXsteps = (XstepCount / xFoV) ;
|
||||
|
||||
// calc steps in Y
|
||||
long YstepCount = abs(Cy - By) ; long numYsteps = (YstepCount / yFoV) ;
|
||||
|
||||
// calc the z changes
|
||||
long XZdeltaCount = Bz - Az ; long XdeltaZperFoV = XZdeltaCount / (numXsteps - 1);
|
||||
long YZdeltaCount = Cz - Bz ; long YdeltaZperFoV = YZdeltaCount / (numYsteps - 1);
|
||||
|
||||
// set the initial delta z to the x one. Later, in the loop, it will be changed continously
|
||||
long deltaZperFoV = XdeltaZperFoV;
|
||||
|
||||
Serial.print(" Ax: "); Serial.print(Ax); Serial.print(" Ay: "); Serial.print(Ay); Serial.print(" Az: "); Serial.print(Az);
|
||||
Serial.print(" Bx: "); Serial.print(Bx); Serial.print(" By: "); Serial.print(By); Serial.print(" Bz: "); Serial.print(Bz);
|
||||
Serial.print(" Cx: "); Serial.print(Cx); Serial.print(" Cy: "); Serial.print(Cy); Serial.print(" Cz: "); Serial.print(Cz);
|
||||
Serial.print(" XdeltaZperFoV: "); Serial.print(XdeltaZperFoV);
|
||||
Serial.print(" YdeltaZperFoV: "); Serial.print(YdeltaZperFoV);
|
||||
Serial.print("#");
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user