Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
c78ec497eThis file is pinned to the selected release. Download the original to open it in your design tool.
//------------------------------------------------------------------------------------------------
// * moveToTarget
// This function is responsible for moving the stage to an absolute location. It takes two
// inputs: an array of floats defining how much to move the stage in each axis and the step size.
// The length of the array should be equal to the number of axes. The step size indicates the
// fraction of a full step being taken and is a float.
//
// ** You will need to change the speed and step sizes in this file for your motors **
void moveToTarget(float target[]){
byte verbose=0;
int ii; //for loop counter
float intendedMove[numAxes]; //How far to move on each axis.
setStep(moveToStepSize);
#ifdef DO_GAMEPAD
PS3.setAllOff(); //Switch off the LEDs to indicate that we're making a non-manual motion
#endif
float oneStep[numAxes]; //how far one step takes us
long stepsToTake[numAxes]; //how many steps to take on each axis
for (ii=0; ii<numAxes; ii++){
if (!axisPresent[ii]){
continue;
}
intendedMove[ii]=target[ii]-stagePosition[ii]; //How far we need to move
/*
//compensate for backlash
if (intendedMove[ii]<0 && digitalRead(stepDir[ii])==1)
intendedMove[ii]=intendedMove[ii]-0.32; //the measured backlash in z
if (intendedMove[ii]>0 && digitalRead(stepDir[ii])==0)
intendedMove[ii]=intendedMove[ii]+0.32; //the measured backlash in z
*/
oneStep[ii] = (fullStep[ii]/360.0) * moveToStepSize * gearRatio[ii];
stepsToTake[ii]=round(intendedMove[ii]/oneStep[ii]); //total number of steps we have to take
if (verbose && !controlViaUSB){
Serial.print(ii+1);
Serial.print(". Targ: ");
Serial.print(target[ii]);
Serial.print("; Pos: ");
Serial.print(stagePosition[ii]);
Serial.print("; Spd: ");
Serial.print(moveToSpeed[ii]);
Serial.print("; oneStp: ");
Serial.print(oneStep[ii]);
Serial.print("; steps2take: ");
Serial.print(stepsToTake[ii]);
Serial.print("; CurPos B4: ");
Serial.println((*mySteppers[ii]).currentPosition() );
}
//Now set the maximum pulse speed on this stepper axis
(*mySteppers[ii]).setSpeed(moveToSpeed[ii]/oneStep[ii]); //seems to be necessary that this is set to a reasonable value
(*mySteppers[ii]).setMaxSpeed(moveToSpeed[ii]/oneStep[ii]);
(*mySteppers[ii]).setAcceleration(moveToAccel[ii]);
if (disableWhenStationary[ii] && abs(intendedMove[ii])>0)
digitalWrite(enable[ii],LOW);
//Update the stage position to what it will be once the move is complete
//No point waiting to do this, since there is no feedback from the stepper
//anyway.
updateStagePos(ii,stepsToTake[ii],moveToStepSize);
//Set up axes for motions
(*mySteppers[ii]).moveTo( (*mySteppers[ii]).currentPosition()+stepsToTake[ii] );
}
//Now run the steppers until they get there
if (verbose && !controlViaUSB){
Serial.println("Moving");
}
runSteppersToPos();
//Return LEDs and Big Easy Drivers to previously selected step size
//This also re-calculates the step size (which we re-defined above).
setPSLEDS();
for (ii=0; ii<numAxes; ii++){
if (!axisPresent[ii])
continue;
#ifdef DO_LCD
lcdStagePos(ii,stagePosition[ii],0);
#endif
stepperPreviousPos[ii]=(*mySteppers[ii]).currentPosition();
(*mySteppers[ii]).setAcceleration(0);
(*mySteppers[ii]).setSpeed(0);
if (disableWhenStationary[ii] && abs(intendedMove[ii])>0)
digitalWrite(enable[ii],HIGH);
}// End for (ii=0; ii<numAxes; ii++){
}//End moveToTarget()
void runSteppersToPos(){
bool verbose=0;
//int n=0; //counter to monitor if the controller is locked in this loop
bool keepMoving=1;
bool axisFinished[numAxes];
for (byte ii=0; ii<numAxes; ii++){
axisFinished[ii]=1;
}
if (verbose){
Serial.println("Entering while loop in runSteppersToPos()");
}
while (keepMoving){
keepMoving=0;
for (byte ii=0; ii<numAxes; ii++){
if (!axisPresent[ii]){
continue;
}
if ((*mySteppers[ii]).distanceToGo() != 0){
keepMoving=1;
axisFinished[ii]=0;
}
}
//Motion is a touch faster if we place run code here
for (byte ii=0; ii<numAxes; ii++){
if (axisFinished[ii] || !axisPresent[ii]){
continue;
}
(*mySteppers[ii]).run();
}
//I haven't seen lock-ups in ages, so we comment out this code.
//Sometimes the controller locks up following analog stick motions
//This is due to it getting stuck in this loop with the motors at zero
//speed. Not yet clear why this happens, but in the mean time we can catch
//it with the following code and deliver an audible indication that something
//is wrong. This enables us to keep going and not lock up.
/* long currentTotalSpeed=0;
for (int ii=0; ii<numAxes; ii++){
if (!axisPresent[ii]){
continue;
}
currentTotalSpeed+=(*mySteppers[ii]).speed();
}
if (currentTotalSpeed==0){
n++;
}
if (n>50){ //If we're stuck here not moving for 50 cycles then something's wrong!
for (int ii=0; ii<5; ii++){
serialBeep();
delay(70);
}
break;
}
*/
} //close while loop
} //End runSteppersToPos