Multiple changes in stepper_motor_controller.
*Change the RATIO for v3 rotator *Add definitions for max speed, max accelaration, baudrate, default home state *Change the Homing function *End stops pin is not float when the end stop is not connected *Add a check if an argument is number in cmd_procmerge-requests/29/head
Agis Zisimatos
parent
fe4163b6cb
commit
9aeb80df55
|
|
@ -3,29 +3,38 @@
|
|||
#include <math.h>
|
||||
#include <AccelStepper.h>
|
||||
|
||||
#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 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 //PIN for step size
|
||||
#define EN 8 //PIN for Enable or Disable Stepper Motors
|
||||
#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_DELAY 60000 //Time to disable the motors in millisecond
|
||||
#define SPR 200 /*Step Per Revolution*/
|
||||
#define RATIO 54 /*Gear ratio*/
|
||||
#define T_DELAY 60000 /*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 180 //Angle scanning multiplier
|
||||
#define MAX_AZ_ANGLE 360 //Maximum Angle of Azimuth for homing scanning
|
||||
#define MAX_EL_ANGLE 360 //Maximum Angle of Elevation for homing scanning
|
||||
#define HOME_AZ 4 /*Homing switch for Azimuth*/
|
||||
#define HOME_EL 5 /*Homing switch for Elevation*/
|
||||
|
||||
#define HOME_DELAY 6000 //Time for homing Decceleration in millisecond
|
||||
#define MAX_AZ_ANGLE 365 /*Maximum Angle of Azimuth for homing scanning*/
|
||||
#define MAX_EL_ANGLE 365 /*Maximum Angle of Elevation for homing scanning*/
|
||||
|
||||
#define MAX_SPEED 100
|
||||
#define MAX_ACCELERATION 50
|
||||
|
||||
#define MIN_PULSE_WIDTH 20 /*in microsecond*/
|
||||
|
||||
#define DEFAULT_HOME_STATE HIGH /*Change to LOW according to Home sensor*/
|
||||
|
||||
#define HOME_DELAY 6000 /*Time for homing Decceleration in millisecond*/
|
||||
|
||||
#define BufferSize 256
|
||||
#define BaudRate 19200
|
||||
|
||||
/*Global Variables*/
|
||||
unsigned long t_DIS = 0; //time to disable the Motors
|
||||
unsigned long t_DIS = 0; /*time to disable the Motors*/
|
||||
/*Define a stepper and the pins it will use*/
|
||||
AccelStepper AZstepper(1, STEP_AZ, DIR_AZ);
|
||||
AccelStepper ELstepper(1, STEP_EL, DIR_EL);
|
||||
|
|
@ -33,26 +42,27 @@ AccelStepper ELstepper(1, STEP_EL, DIR_EL);
|
|||
void setup()
|
||||
{
|
||||
/*Change these to suit your stepper if you want*/
|
||||
AZstepper.setMaxSpeed(150);
|
||||
AZstepper.setAcceleration(50);
|
||||
|
||||
AZstepper.setMaxSpeed(MAX_SPEED);
|
||||
AZstepper.setAcceleration(MAX_ACCELERATION);
|
||||
/*Change these to suit your stepper if you want*/
|
||||
ELstepper.setMaxSpeed(150);
|
||||
ELstepper.setAcceleration(50);
|
||||
|
||||
ELstepper.setMaxSpeed(MAX_SPEED);
|
||||
ELstepper.setAcceleration(MAX_ACCELERATION);
|
||||
/*Set minimum pulse width*/
|
||||
AZstepper.setMinPulseWidth(MIN_PULSE_WIDTH);
|
||||
ELstepper.setMinPulseWidth(MIN_PULSE_WIDTH);
|
||||
/*Enable Motors*/
|
||||
pinMode(EN, OUTPUT);
|
||||
digitalWrite(EN, LOW);
|
||||
/*Step size*/
|
||||
pinMode(MS1, OUTPUT);
|
||||
digitalWrite(MS1, LOW); //Full step
|
||||
digitalWrite(MS1, LOW); /*Full step*/
|
||||
/*Homing switch*/
|
||||
pinMode(HOME_AZ, INPUT);
|
||||
pinMode(HOME_EL, INPUT);
|
||||
pinMode(HOME_AZ, INPUT_PULLUP);
|
||||
pinMode(HOME_EL, INPUT_PULLUP);
|
||||
/*Serial Communication*/
|
||||
Serial.begin(19200);
|
||||
Serial.begin(BaudRate);
|
||||
/*Initial Homing*/
|
||||
Homing(deg2step(-ANGLE_SCANNING_MULT), deg2step(-ANGLE_SCANNING_MULT));
|
||||
Homing(deg2step(-MAX_AZ_ANGLE), deg2step(-MAX_EL_ANGLE));
|
||||
}
|
||||
|
||||
void loop()
|
||||
|
|
@ -66,9 +76,7 @@ void loop()
|
|||
|
||||
/*Disable Motors*/
|
||||
if (AZstep == AZstepper.currentPosition() && ELstep == ELstepper.currentPosition() && millis()-t_DIS > T_DELAY)
|
||||
{
|
||||
digitalWrite(EN, HIGH);
|
||||
}
|
||||
/*Enable Motors*/
|
||||
else
|
||||
digitalWrite(EN, LOW);
|
||||
|
|
@ -82,10 +90,8 @@ void loop()
|
|||
/*Homing Function*/
|
||||
void Homing(int AZsteps, int ELsteps)
|
||||
{
|
||||
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 value_Home_AZ = DEFAULT_HOME_STATE;
|
||||
int value_Home_EL = DEFAULT_HOME_STATE;
|
||||
boolean isHome_AZ = false;
|
||||
boolean isHome_EL = false;
|
||||
|
||||
|
|
@ -96,41 +102,28 @@ void Homing(int AZsteps, int ELsteps)
|
|||
{
|
||||
value_Home_AZ = digitalRead(HOME_AZ);
|
||||
value_Home_EL = digitalRead(HOME_EL);
|
||||
/* Change to LOW according to Home sensor */
|
||||
if (value_Home_AZ == HIGH)
|
||||
/*Change to LOW according to Home sensor*/
|
||||
if (value_Home_AZ == DEFAULT_HOME_STATE)
|
||||
{
|
||||
AZstepper.moveTo(AZstepper.currentPosition());
|
||||
isHome_AZ = true;
|
||||
}
|
||||
/* Change to LOW according to Home sensor */
|
||||
if (value_Home_EL == HIGH)
|
||||
/*Change to LOW according to Home sensor*/
|
||||
if (value_Home_EL == DEFAULT_HOME_STATE)
|
||||
{
|
||||
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);
|
||||
error(0);
|
||||
break;
|
||||
}
|
||||
if (ELstepper.distanceToGo() == 0 && !isHome_EL)
|
||||
{
|
||||
error(1);
|
||||
break;
|
||||
}
|
||||
|
||||
AZstepper.run();
|
||||
ELstepper.run();
|
||||
}
|
||||
|
|
@ -150,14 +143,14 @@ void Homing(int AZsteps, int ELsteps)
|
|||
void cmd_proc(int &stepAz, int &stepEl)
|
||||
{
|
||||
/*Serial*/
|
||||
char buffer[256];
|
||||
char buffer[BufferSize];
|
||||
char incomingByte;
|
||||
char *p=buffer;
|
||||
char *str;
|
||||
static int counter=0;
|
||||
char *Data = buffer;
|
||||
char *rawData;
|
||||
static int BufferCnt = 0;
|
||||
char data[100];
|
||||
|
||||
double angleAz,angleEl;
|
||||
double angleAz, angleEl;
|
||||
|
||||
/*Read from serial*/
|
||||
while (Serial.available() > 0)
|
||||
|
|
@ -166,7 +159,7 @@ void cmd_proc(int &stepAz, int &stepEl)
|
|||
/* XXX: Get position using custom and test code */
|
||||
if (incomingByte == '!')
|
||||
{
|
||||
/* Get position */
|
||||
/*Get position*/
|
||||
Serial.print("TM");
|
||||
Serial.print(1);
|
||||
Serial.print(" ");
|
||||
|
|
@ -179,13 +172,12 @@ void cmd_proc(int &stepAz, int &stepEl)
|
|||
/*new data*/
|
||||
else if (incomingByte == '\n')
|
||||
{
|
||||
p = buffer;
|
||||
buffer[counter] = 0;
|
||||
buffer[BufferCnt] = 0;
|
||||
if (buffer[0] == 'A' && buffer[1] == 'Z')
|
||||
{
|
||||
if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L')
|
||||
{
|
||||
/* Get position */
|
||||
/*Get position*/
|
||||
Serial.print("AZ");
|
||||
Serial.print(step2deg(AZstepper.currentPosition()), 1);
|
||||
Serial.print(" ");
|
||||
|
|
@ -196,27 +188,32 @@ void cmd_proc(int &stepAz, int &stepEl)
|
|||
else
|
||||
{
|
||||
/*Get the absolute value of angle*/
|
||||
str = strtok_r(p, " " , &p);
|
||||
strncpy(data, str+2, 10);
|
||||
angleAz = atof(data);
|
||||
/*Calculate the steps*/
|
||||
stepAz = deg2step(angleAz);
|
||||
|
||||
/*Get the absolute value of angle*/
|
||||
str = strtok_r(p, " " , &p);
|
||||
if (str[0] == 'E' && str[1] == 'L')
|
||||
rawData = strtok_r(Data, " " , &Data);
|
||||
strncpy(data, rawData+2, 10);
|
||||
if (isNumber(data))
|
||||
{
|
||||
strncpy(data, str+2, 10);
|
||||
angleEl = atof(data);
|
||||
angleAz = atof(data);
|
||||
/*Calculate the steps*/
|
||||
stepEl = deg2step(angleEl);
|
||||
stepAz = deg2step(angleAz);
|
||||
}
|
||||
/*Get the absolute value of angle*/
|
||||
rawData = strtok_r(Data, " " , &Data);
|
||||
if (rawData[0] == 'E' && rawData[1] == 'L')
|
||||
{
|
||||
strncpy(data, rawData+2, 10);
|
||||
if (isNumber(data))
|
||||
{
|
||||
angleEl = atof(data);
|
||||
/*Calculate the steps*/
|
||||
stepEl = deg2step(angleEl);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
/* Stop Moving */
|
||||
/*Stop Moving*/
|
||||
else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E')
|
||||
{
|
||||
/* Get position */
|
||||
/*Get position*/
|
||||
Serial.print("AZ");
|
||||
Serial.print(step2deg(AZstepper.currentPosition()), 1);
|
||||
Serial.print(" ");
|
||||
|
|
@ -225,29 +222,29 @@ void cmd_proc(int &stepAz, int &stepEl)
|
|||
stepAz = AZstepper.currentPosition();
|
||||
stepEl = ELstepper.currentPosition();
|
||||
}
|
||||
/* Reset the rotator */
|
||||
/*Reset the rotator*/
|
||||
else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T')
|
||||
{
|
||||
/* Get position */
|
||||
/*Get position*/
|
||||
Serial.print("AZ");
|
||||
Serial.print(step2deg(AZstepper.currentPosition()), 1);
|
||||
Serial.print(" ");
|
||||
Serial.print("EL");
|
||||
Serial.println(step2deg(ELstepper.currentPosition()), 1);
|
||||
/*Move the steppers to initial position*/
|
||||
Homing(0,0);
|
||||
Homing(deg2step(-MAX_AZ_ANGLE), deg2step(-MAX_EL_ANGLE));
|
||||
/*Zero the steps*/
|
||||
stepAz = 0;
|
||||
stepEl = 0;
|
||||
}
|
||||
counter = 0;
|
||||
BufferCnt = 0;
|
||||
/*Reset the disable motor time*/
|
||||
t_DIS = 0;
|
||||
}
|
||||
/*Fill the buffer with incoming data*/
|
||||
else {
|
||||
buffer[counter] = incomingByte;
|
||||
counter++;
|
||||
buffer[BufferCnt] = incomingByte;
|
||||
BufferCnt++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -301,3 +298,14 @@ double step2deg(int Step)
|
|||
{
|
||||
return(360.00*Step/(SPR*RATIO));
|
||||
}
|
||||
|
||||
/*Check if is argument in number*/
|
||||
boolean isNumber(char *input)
|
||||
{
|
||||
for (int i = 0; input[i] != '\0'; i++)
|
||||
{
|
||||
if (isalpha(input[i]))
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue