add homing procedure
This commit is contained in:
parent
5c1aa24973
commit
93435ebccc
1 changed files with 317 additions and 37 deletions
|
|
@ -17,19 +17,119 @@
|
|||
|
||||
#define PIN_LIGHT D6
|
||||
|
||||
AccelStepper stepperPan(AccelStepper::DRIVER, PAN_STEP_PIN, PAN_DIR_PIN);
|
||||
|
||||
// Motor / Hardware Configuration
|
||||
int microsteppingPan = 4;
|
||||
long stepsPerRotationPan=microsteppingPan*85*400/17; //17z to 85z
|
||||
long homepos_pan=-stepsPerRotationPan/360*4; //positive number=homed position is too far clockwise
|
||||
|
||||
int microsteppingTilt = 4;
|
||||
|
||||
long stepsPerRotationTilt=microsteppingTilt*42*400/12; //12z to 42z
|
||||
long homepos_tilt=0; //positive number=homed position is too far clockwise
|
||||
|
||||
|
||||
enum Mode{
|
||||
PROGRAM,
|
||||
HOME,
|
||||
RUN,
|
||||
IDLE
|
||||
};
|
||||
enum Mode mode=PROGRAM;
|
||||
|
||||
|
||||
|
||||
enum Stepperpreset{
|
||||
HOMECOARSE,
|
||||
HOMEFINE,
|
||||
SLOW,
|
||||
NORMAL,
|
||||
FAST
|
||||
};
|
||||
|
||||
enum Stepper{
|
||||
PAN,
|
||||
TILT
|
||||
};
|
||||
|
||||
bool homedPan=false;
|
||||
bool homedTilt=false;
|
||||
|
||||
|
||||
AccelStepper stepperPan(AccelStepper::DRIVER, PAN_STEP_PIN, PAN_DIR_PIN);
|
||||
AccelStepper stepperTilt(AccelStepper::DRIVER, TILT_STEP_PIN, TILT_DIR_PIN);
|
||||
|
||||
unsigned int microsteppingPan = 4;
|
||||
unsigned long stepsPerRotationPan=microsteppingPan*42*400/12; //12z to 42z, 16x microstepping
|
||||
|
||||
unsigned int microsteppingTilt = 4;
|
||||
unsigned long stepsPerRotationTilt=microsteppingTilt*85*400/17;//17z to 85z, 16x microstepping
|
||||
void setEnable(bool t);
|
||||
bool getEnable();
|
||||
void setStepperPreset(Stepperpreset preset,Stepper s);
|
||||
void setMode(Mode newmode);
|
||||
bool home(bool direction,bool endstop, uint8_t& homestage, Stepper s);
|
||||
|
||||
void testMovementPan();
|
||||
void testMovementTilt();
|
||||
bool isEndstopPan();
|
||||
bool isEndstopTilt();
|
||||
|
||||
|
||||
void setEnable(bool t){
|
||||
digitalWrite(EN_PIN,!t); //active low
|
||||
}
|
||||
bool getEnable() {
|
||||
return !digitalRead(EN_PIN);
|
||||
}
|
||||
|
||||
void setStepperPreset(Stepperpreset preset,Stepper s)
|
||||
{
|
||||
float pan_speed=0;
|
||||
float pan_accel=0;
|
||||
float tilt_speed=0;
|
||||
float tilt_accel=0;
|
||||
switch (preset){
|
||||
case HOMECOARSE:
|
||||
pan_speed=stepsPerRotationPan*1/10;
|
||||
pan_accel=stepsPerRotationPan*1/4;
|
||||
tilt_speed=stepsPerRotationTilt*1/10;
|
||||
tilt_accel=stepsPerRotationTilt*1/4;
|
||||
break;
|
||||
case HOMEFINE:
|
||||
pan_speed=stepsPerRotationPan*1/400;
|
||||
pan_accel=stepsPerRotationPan*1/4;
|
||||
tilt_speed=stepsPerRotationTilt*1/400;
|
||||
tilt_accel=stepsPerRotationTilt*1/4;
|
||||
break;
|
||||
case SLOW:
|
||||
pan_speed=stepsPerRotationPan*1/10;
|
||||
pan_accel=stepsPerRotationPan*1/4;
|
||||
tilt_speed=stepsPerRotationTilt*1/10;
|
||||
tilt_accel=stepsPerRotationTilt*1/4;
|
||||
break;
|
||||
case NORMAL:
|
||||
pan_speed=stepsPerRotationPan*1/4;
|
||||
pan_accel=stepsPerRotationPan*1/2;
|
||||
tilt_speed=stepsPerRotationTilt*1/4;
|
||||
tilt_accel=stepsPerRotationTilt*1/2;
|
||||
break;
|
||||
case FAST:
|
||||
//abs. max. speed w/o load: stepsPerRotationPan*2/3 @19V
|
||||
//abs. max. speed w/o load: stepsPerRotationTilt*1/2 @19V
|
||||
pan_speed=stepsPerRotationPan*1/2;
|
||||
pan_accel=stepsPerRotationPan*2/1;
|
||||
tilt_speed=stepsPerRotationTilt*1/2;
|
||||
tilt_accel=stepsPerRotationTilt*2/1;
|
||||
break;
|
||||
}
|
||||
|
||||
if (s==PAN) {
|
||||
stepperPan.setMaxSpeed(pan_speed);
|
||||
stepperPan.setAcceleration(pan_accel);
|
||||
Serial.print("Set Pan Preset. Speed=");Serial.print(pan_speed); Serial.print(" acc="); Serial.println(pan_accel);
|
||||
}else if (s==TILT){
|
||||
stepperTilt.setMaxSpeed(tilt_speed);
|
||||
stepperTilt.setAcceleration(tilt_accel);
|
||||
Serial.print("Set Tilt Preset. Speed=");Serial.print(tilt_speed); Serial.print(" acc="); Serial.println(tilt_accel);
|
||||
}
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
|
|
@ -41,50 +141,230 @@ void setup() {
|
|||
digitalWrite(PIN_LIGHT, LOW);
|
||||
setEnable(false);
|
||||
|
||||
stepperPan.setMaxSpeed(2000); //tested w/o load 15000 @16 microsteps
|
||||
stepperPan.setAcceleration(5000.0); //tested w/o load 80000 @16 microsteps
|
||||
stepperPan.moveTo(stepsPerRotationPan/2);
|
||||
|
||||
//stepperPan.moveTo(stepsPerRotationPan/2);
|
||||
|
||||
stepperTilt.setMaxSpeed(2000); //tested w/o load 15000 @16 microsteps
|
||||
stepperTilt.setAcceleration(5000.0); //tested w/o load 80000 @16 microsteps
|
||||
stepperTilt.moveTo(stepsPerRotationTilt/4);
|
||||
//stepperTilt.moveTo(stepsPerRotationTilt/4);
|
||||
|
||||
setEnable(true);
|
||||
delay(1000); //wait until terminal ready
|
||||
if (isEndstopPan()) {
|
||||
delay(500);
|
||||
setMode(PROGRAM);
|
||||
|
||||
}else{
|
||||
setMode(HOME);
|
||||
}
|
||||
|
||||
Serial.println("Starting");
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
static unsigned long last_change;
|
||||
|
||||
if (stepperPan.distanceToGo() == 0) {
|
||||
if (stepperPan.currentPosition()>100) {
|
||||
Serial.print("Pan Moving to: "); Serial.println(0);
|
||||
stepperPan.moveTo(0);
|
||||
}else{
|
||||
Serial.print("Pan Moving to: "); Serial.println(stepsPerRotationPan/2);
|
||||
stepperPan.moveTo(stepsPerRotationPan/2);
|
||||
}
|
||||
// ### Mode handling ###
|
||||
switch(mode){
|
||||
case PROGRAM:
|
||||
break;
|
||||
|
||||
case HOME:
|
||||
static uint8_t homestagePan=0;
|
||||
static uint8_t homestageTilt=0;
|
||||
homedPan=home(1,isEndstopPan(),homestagePan,PAN);
|
||||
homedTilt=home(1,isEndstopTilt(),homestageTilt,TILT);
|
||||
if (homedPan && homedTilt) { //finished homing
|
||||
setMode(RUN);
|
||||
}
|
||||
break;
|
||||
|
||||
case RUN:
|
||||
testMovementPan();
|
||||
testMovementTilt();
|
||||
break;
|
||||
|
||||
case IDLE:
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
if (stepperTilt.distanceToGo() == 0) {
|
||||
if (stepperTilt.currentPosition()>100) {
|
||||
Serial.print("Tilt Moving to: "); Serial.println(0);
|
||||
stepperTilt.moveTo(0);
|
||||
}else{
|
||||
Serial.print("Tilt Moving to: "); Serial.println(stepsPerRotationTilt/4);
|
||||
stepperTilt.moveTo(stepsPerRotationTilt/4);
|
||||
}
|
||||
}
|
||||
|
||||
bool resultPan = stepperPan.run();
|
||||
bool resultTilt = stepperTilt.run();
|
||||
|
||||
|
||||
if ( millis() > last_change+500 ) {
|
||||
if (getEnable()){
|
||||
//bool resultPan = stepperPan.run();
|
||||
stepperPan.run();
|
||||
//bool resultTilt = stepperTilt.run();
|
||||
stepperTilt.run();
|
||||
}
|
||||
|
||||
|
||||
// ### TIMED PRINTING ###
|
||||
|
||||
if ( millis() > last_change+250 ) {
|
||||
last_change=millis();
|
||||
//Serial.print("run="); Serial.print(result);
|
||||
//Serial.print(" dist="); Serial.print(stepperPan.distanceToGo());
|
||||
//Serial.print(" pos="); Serial.print(stepperPan.currentPosition());
|
||||
//Serial.println();
|
||||
|
||||
|
||||
switch(mode){
|
||||
case PROGRAM:
|
||||
Serial.print("PAN Endstop="); Serial.print(digitalRead(ENDSTOP_PAN));
|
||||
Serial.print(" TILT Endstop="); Serial.print(analogRead(ENDSTOP_TILT));
|
||||
Serial.println();
|
||||
break;
|
||||
|
||||
case HOME:
|
||||
break;
|
||||
|
||||
case RUN:
|
||||
testMovementPan();
|
||||
|
||||
Serial.print("Pan pos="); Serial.print(stepperPan.currentPosition());
|
||||
Serial.print(" Tilt pos="); Serial.print(stepperTilt.currentPosition());
|
||||
Serial.println();
|
||||
break;
|
||||
|
||||
case IDLE:
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void setMode(Mode newmode) {
|
||||
switch(newmode){
|
||||
case PROGRAM:
|
||||
setEnable(false);
|
||||
homedPan=false;
|
||||
homedTilt=false;
|
||||
mode=newmode;
|
||||
Serial.println("Mode set to PROGRAM");
|
||||
break;
|
||||
|
||||
case HOME:
|
||||
setStepperPreset(HOMECOARSE,PAN); //set Pan
|
||||
setStepperPreset(HOMECOARSE,TILT); //set Tilt
|
||||
setEnable(true);
|
||||
mode=newmode;
|
||||
Serial.println("Mode set to HOME");
|
||||
break;
|
||||
|
||||
case RUN:
|
||||
setStepperPreset(FAST,PAN); //set Pan
|
||||
setStepperPreset(FAST,TILT); //set Tilt
|
||||
setEnable(true);
|
||||
mode=newmode;
|
||||
Serial.println("Mode set to RUN");
|
||||
break;
|
||||
|
||||
case IDLE:
|
||||
mode=newmode;
|
||||
Serial.println("Mode set to IDLE");
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool home(bool direction,bool endstop, uint8_t& homestage, Stepper s){
|
||||
//PAN direction = 1 -> clockwise (viewed from top)
|
||||
//TILT direction = 1 -> clockwise (viewed pcb side)
|
||||
AccelStepper* stepper;
|
||||
if (s==PAN) {
|
||||
stepper=&stepperPan;
|
||||
}else if (s==TILT) {
|
||||
stepper=&stepperTilt;
|
||||
}
|
||||
|
||||
switch (homestage){
|
||||
case 0: //move fast until endstop triggered
|
||||
if (!endstop) {
|
||||
stepper->move(direction*2*stepper->acceleration());
|
||||
}else{ //endstop reached
|
||||
stepper->stop();
|
||||
setStepperPreset(HOMECOARSE,s);
|
||||
homestage=1;
|
||||
Serial.println("Endstop Reached with coarse");
|
||||
}
|
||||
|
||||
break;
|
||||
case 1:
|
||||
if (endstop) {
|
||||
stepper->move(-direction*2*stepper->acceleration()); //move away
|
||||
}else{ //endstop released
|
||||
stepper->stop();
|
||||
setStepperPreset(HOMEFINE,s);
|
||||
homestage=2;
|
||||
Serial.println("Endstop Released");
|
||||
}
|
||||
|
||||
break;
|
||||
case 2:
|
||||
if (!endstop) {
|
||||
stepper->move(direction*2*stepper->acceleration()); //move towards endstop
|
||||
}else{ //endstop reached
|
||||
stepper->stop();
|
||||
Serial.println("Endstop Reached with fine");
|
||||
if (s==PAN) {
|
||||
stepper->setCurrentPosition(homepos_pan+stepsPerRotationPan);
|
||||
}else if(s==TILT) {
|
||||
stepper->setCurrentPosition(homepos_tilt+stepsPerRotationTilt);
|
||||
}
|
||||
|
||||
homestage=3;
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void testMovementPan()
|
||||
{
|
||||
static long last_positionReached=0;
|
||||
if (stepperPan.distanceToGo() == 0 && last_positionReached==0) {
|
||||
last_positionReached=millis();
|
||||
}
|
||||
if (last_positionReached!=0 && millis()-last_positionReached>200) {
|
||||
last_positionReached=0;
|
||||
|
||||
if (stepperPan.currentPosition()<stepsPerRotationPan-100) {
|
||||
stepperPan.moveTo(stepsPerRotationPan);
|
||||
}else{
|
||||
stepperPan.moveTo(stepsPerRotationPan-random(200,stepsPerRotationPan));
|
||||
}
|
||||
}
|
||||
}
|
||||
void testMovementTilt()
|
||||
{
|
||||
|
||||
static long last_positionReached=0;
|
||||
if (stepperTilt.distanceToGo() == 0 && last_positionReached==0) {
|
||||
last_positionReached=millis();
|
||||
}
|
||||
if (last_positionReached!=0 && millis()-last_positionReached>1000) {
|
||||
last_positionReached=0;
|
||||
|
||||
if (stepperTilt.currentPosition()<stepsPerRotationTilt-100) {
|
||||
stepperTilt.moveTo(stepsPerRotationTilt);
|
||||
}else{
|
||||
stepperTilt.moveTo(stepsPerRotationTilt-random(200,stepsPerRotationTilt));
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool isEndstopPan()
|
||||
{
|
||||
//PAN 0V when magnet is close, High when not
|
||||
return !digitalRead(ENDSTOP_PAN); //active Low
|
||||
}
|
||||
bool isEndstopTilt()
|
||||
{
|
||||
// Tilt Hall sensor is U18. 0V when powered on. switches when magnetic field changes (keeps state when magnet not close).
|
||||
return analogRead(ENDSTOP_TILT)>=512; //active when magnet passed by (field changed to north? "red marking").
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue