Add Initial Homing function in arduino code.
Agis Zisimatos
parent
1794abb860
commit
f2fcda9133
|
|
@ -3,17 +3,26 @@
|
|||
#include <math.h>
|
||||
#include <AccelStepper.h>
|
||||
|
||||
#define DIR_AZ 18
|
||||
#define STEP_AZ 10
|
||||
#define DIR_EL 6
|
||||
#define STEP_EL 7
|
||||
#define DIR_AZ 18 //PIN for Azimuth Direction
|
||||
#define STEP_AZ 10 //PIN for Azimuth Steps
|
||||
#define DIR_EL 6 //PIN for Elevation Direction
|
||||
#define STEP_EL 7 //PIN for Elevation Steps
|
||||
|
||||
#define MS1 9
|
||||
#define EN 8
|
||||
#define MS1 9 //PIN for step size
|
||||
#define EN 8 //PIN for Enable or Disable Stepper Motors
|
||||
|
||||
#define SPR 200 //Step Per Revolution
|
||||
#define RATIO 60 //Gear ratio
|
||||
#define T_DEALY 20000 //Time to disable the motors
|
||||
#define T_DELAY 20000 //Time to disable the motors in millisecond
|
||||
|
||||
#define HOME_AZ 4 //Homing switch for Azimuth
|
||||
#define HOME_EL 5 //Homing switch for Elevation
|
||||
/*The MAX_ANGLE depends of ANGLE_SCANNING_MULT and maybe misbehave for large values*/
|
||||
#define ANGLE_SCANNING_MULT 10 //Angle scanning multiplier
|
||||
#define MAX_AZ_ANGLE 200 //Maximum Angle of Azimuth for homing scanning
|
||||
#define MAX_EL_ANGLE 100 //Maximum Angle of Elevation for homing scanning
|
||||
|
||||
#define HOME_DELAY 6000 //Time for homing Decceleration in millisecond
|
||||
|
||||
/*Global Variables*/
|
||||
unsigned long t_DIS = 0; //time to disable the Motors
|
||||
|
|
@ -25,12 +34,12 @@ AccelStepper ELstepper(1, STEP_EL, DIR_EL);
|
|||
void setup()
|
||||
{
|
||||
/*Change these to suit your stepper if you want*/
|
||||
AZstepper.setMaxSpeed(400);
|
||||
AZstepper.setAcceleration(100);
|
||||
AZstepper.setMaxSpeed(200);
|
||||
AZstepper.setAcceleration(200);
|
||||
|
||||
/*Change these to suit your stepper if you want*/
|
||||
ELstepper.setMaxSpeed(400);
|
||||
ELstepper.setAcceleration(100);
|
||||
ELstepper.setAcceleration(200);
|
||||
|
||||
/*Enable Motors*/
|
||||
pinMode(EN, OUTPUT);
|
||||
|
|
@ -38,8 +47,13 @@ void setup()
|
|||
/*Step size*/
|
||||
pinMode(MS1, OUTPUT);
|
||||
digitalWrite(MS1, LOW); //Full step
|
||||
/*Homing switch*/
|
||||
pinMode(HOME_AZ, INPUT);
|
||||
pinMode(HOME_EL, INPUT);
|
||||
/*Serial Communication*/
|
||||
Serial.begin(19200);
|
||||
/*Initial Homing*/
|
||||
Initial_Homing();
|
||||
}
|
||||
|
||||
void loop()
|
||||
|
|
@ -53,7 +67,7 @@ void loop()
|
|||
t_DIS = millis();
|
||||
|
||||
/*Disable Motors*/
|
||||
if (AZstep == AZstepper.currentPosition() && ELstep == ELstepper.currentPosition() && millis()-t_DIS > T_DEALY)
|
||||
if (AZstep == AZstepper.currentPosition() && ELstep == ELstepper.currentPosition() && millis()-t_DIS > T_DELAY)
|
||||
digitalWrite(EN, HIGH);
|
||||
/*Enable Motors*/
|
||||
else
|
||||
|
|
@ -65,6 +79,73 @@ void loop()
|
|||
stepper_move(AZstep, ELstep);
|
||||
}
|
||||
|
||||
void Initial_Homing()
|
||||
{
|
||||
int value_Home_AZ = HIGH;
|
||||
int value_Home_EL = HIGH;
|
||||
int n_AZ = 1; //Times that AZ angle has changed
|
||||
int n_EL = 1; //Times that EL angle has changed
|
||||
int AZsteps = deg2step(-n_AZ*ANGLE_SCANNING_MULT); //Initial scanning steps for AZ
|
||||
int ELsteps = deg2step(-n_EL*ANGLE_SCANNING_MULT); //Initial scanning steps for EL
|
||||
boolean isHome_AZ = false;
|
||||
boolean isHome_EL = false;
|
||||
|
||||
AZstepper.moveTo(AZsteps);
|
||||
ELstepper.moveTo(ELsteps);
|
||||
|
||||
while(isHome_AZ == false || isHome_EL == false)
|
||||
{
|
||||
value_Home_AZ = digitalRead(HOME_AZ);
|
||||
value_Home_EL = digitalRead(HOME_EL);
|
||||
|
||||
if (value_Home_AZ == LOW)
|
||||
{
|
||||
AZstepper.moveTo(AZstepper.currentPosition());
|
||||
isHome_AZ = true;
|
||||
}
|
||||
if (value_Home_EL == LOW)
|
||||
{
|
||||
ELstepper.moveTo(ELstepper.currentPosition());
|
||||
isHome_EL = true;
|
||||
}
|
||||
if (AZstepper.distanceToGo() == 0 && !isHome_AZ)
|
||||
{
|
||||
n_AZ++;
|
||||
AZsteps = deg2step(pow(-1,n_AZ)*n_AZ*ANGLE_SCANNING_MULT);
|
||||
if (abs(n_AZ*ANGLE_SCANNING_MULT) > MAX_AZ_ANGLE)
|
||||
{
|
||||
error(0);
|
||||
break;
|
||||
}
|
||||
AZstepper.moveTo(AZsteps);
|
||||
}
|
||||
if (ELstepper.distanceToGo() == 0 && !isHome_EL)
|
||||
{
|
||||
n_EL++;
|
||||
ELsteps = deg2step(pow(-1,n_EL)*n_EL*ANGLE_SCANNING_MULT);
|
||||
if (abs(n_EL*ANGLE_SCANNING_MULT) > MAX_EL_ANGLE)
|
||||
{
|
||||
error(1);
|
||||
break;
|
||||
}
|
||||
ELstepper.moveTo(ELsteps);
|
||||
}
|
||||
|
||||
AZstepper.run();
|
||||
ELstepper.run();
|
||||
}
|
||||
/*Delay to Deccelerate*/
|
||||
long time = millis();
|
||||
while(millis() - time < HOME_DELAY)
|
||||
{
|
||||
AZstepper.run();
|
||||
ELstepper.run();
|
||||
}
|
||||
/*Reset the steps*/
|
||||
AZstepper.setCurrentPosition(0);
|
||||
ELstepper.setCurrentPosition(0);
|
||||
}
|
||||
|
||||
/*EasyComm 2 Protocol & Calculate the steps*/
|
||||
void cmd_proc(int &stepAz, int &stepEl)
|
||||
{
|
||||
|
|
@ -118,6 +199,34 @@ void cmd_proc(int &stepAz, int &stepEl)
|
|||
}
|
||||
}
|
||||
|
||||
/*Error Handling*/
|
||||
void error(int num_error)
|
||||
{
|
||||
switch (num_error)
|
||||
{
|
||||
/*Azimuth error*/
|
||||
case (0):
|
||||
while(1)
|
||||
{
|
||||
Serial.println("Error[0]: Azimuth Homing");
|
||||
delay(100);
|
||||
}
|
||||
/*Elevation error*/
|
||||
case (1):
|
||||
while(1)
|
||||
{
|
||||
Serial.println("Error[1]: Elevation Homing");
|
||||
delay(100);
|
||||
}
|
||||
default:
|
||||
while(1)
|
||||
{
|
||||
Serial.println("Error[?]: Unknown error");
|
||||
delay(100);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*Send pulses to stepper motor drivers*/
|
||||
void stepper_move(int stepAz, int stepEl)
|
||||
{
|
||||
|
|
@ -138,4 +247,4 @@ int deg2step(double deg)
|
|||
double step2deg(int Step)
|
||||
{
|
||||
return(360*Step/(SPR*RATIO));
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue