From 9aeb80df55a51f698cd7be72714b20daee3b01bb Mon Sep 17 00:00:00 2001 From: Agis Zisimatos Date: Mon, 3 Aug 2015 17:11:55 +0300 Subject: [PATCH] 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_proc --- .../stepper_motor_controller.ino | 182 +++++++++--------- 1 file changed, 95 insertions(+), 87 deletions(-) diff --git a/Arduino/stepper_motor_controller/stepper_motor_controller.ino b/Arduino/stepper_motor_controller/stepper_motor_controller.ino index 2c54168..d848e88 100644 --- a/Arduino/stepper_motor_controller/stepper_motor_controller.ino +++ b/Arduino/stepper_motor_controller/stepper_motor_controller.ino @@ -3,29 +3,38 @@ #include #include -#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; +}