173 lines
6.0 KiB
C++
173 lines
6.0 KiB
C++
/******************************************************************************/
|
|
/******************************************************************************/
|
|
/******************************************************************************/
|
|
#define SAMPLE_TIME 0.1 /* Control loop in s */
|
|
#define RATIO 54 /* Gear ratio */
|
|
#define MIN_PULSE_WIDTH 20 /* In microsecond for AccelStepper */
|
|
#define MAX_SPEED 200 /* In steps/s */
|
|
#define MAX_ACCELERATION 80 /* In steps/s^2 */
|
|
#define SPR 200 /* Step Per Revolution */
|
|
#define MIN_M1_ANGLE 0 /* Minimum angle of azimuth */
|
|
#define MAX_M1_ANGLE 360 /* Maximum angle of azimuth */
|
|
#define MIN_M2_ANGLE 0 /* Minimum angle of elevation */
|
|
#define MAX_M2_ANGLE 90 /* Maximum angle of elevation */
|
|
#define DEFAULT_HOME_STATE HIGH /* Change to LOW according to Home sensor */
|
|
#define HOME_DELAY 10000 /* Time for homing Deceleration in millisecond */
|
|
/******************************************************************************/
|
|
/******************************************************************************/
|
|
/******************************************************************************/
|
|
#include <AccelStepper.h>
|
|
#include <Wire.h>
|
|
#include "../libraries/globals.h"
|
|
#include "../libraries/easycomm.h"
|
|
#include "../libraries/rotator_pins.h"
|
|
#include "../libraries/rs485.h"
|
|
#include "../libraries/endstop.h"
|
|
#include "../libraries/watchdog.h"
|
|
/******************************************************************************/
|
|
uint32_t t_run = 0;
|
|
easycomm comm;
|
|
AccelStepper stepper_az(1, M1IN1, M1IN2);
|
|
AccelStepper stepper_el(1, M2IN1, M2IN2);
|
|
endstop switch_az(SW1, DEFAULT_HOME_STATE), switch_el(SW2, DEFAULT_HOME_STATE);
|
|
wdt_timer wdt;
|
|
/******************************************************************************/
|
|
enum _rotator_error homing(int32_t seek_az, int32_t seek_el);
|
|
int32_t deg2step(float deg);
|
|
float step2deg(int32_t step);
|
|
/******************************************************************************/
|
|
|
|
void setup() {
|
|
/* Homing switch */
|
|
switch_az.init();
|
|
switch_el.init();
|
|
|
|
/* Serial Communication */
|
|
comm.easycomm_init();
|
|
|
|
/* Stepper Motor setup */
|
|
stepper_az.setEnablePin(MOTOR_EN);
|
|
stepper_az.setPinsInverted(false, false, true);
|
|
stepper_az.enableOutputs();
|
|
stepper_az.setMaxSpeed(MAX_SPEED);
|
|
stepper_az.setAcceleration(MAX_ACCELERATION);
|
|
stepper_az.setMinPulseWidth(MIN_PULSE_WIDTH);
|
|
stepper_el.setPinsInverted(false, false, true);
|
|
stepper_el.enableOutputs();
|
|
stepper_el.setMaxSpeed(MAX_SPEED);
|
|
stepper_el.setAcceleration(MAX_ACCELERATION);
|
|
stepper_el.setMinPulseWidth(MIN_PULSE_WIDTH);
|
|
|
|
/* Initialize WDT */
|
|
wdt.watchdog_init();
|
|
}
|
|
|
|
void loop() {
|
|
/* Update WDT */
|
|
wdt.watchdog_reset();
|
|
|
|
/* Get end stop status */
|
|
rotator.switch_az = switch_az.get_state();
|
|
rotator.switch_el = switch_el.get_state();
|
|
|
|
/*Read the steps from serial*/
|
|
comm.easycomm_proc();
|
|
|
|
/* Get positions */
|
|
control_az.input = step2deg(stepper_az.currentPosition());
|
|
control_el.input = step2deg(stepper_el.currentPosition());
|
|
|
|
/* Check rotator status */
|
|
if (rotator.rotator_status != error) {
|
|
if (rotator.homing_flag == false) {
|
|
rotator.control_mode = position;
|
|
rotator.rotator_error = homing(deg2step(-MAX_M1_ANGLE),
|
|
deg2step(-MAX_M2_ANGLE));
|
|
if (rotator.rotator_error == no_error) {
|
|
/*Zero the steps*/
|
|
rotator.rotator_status = idle;
|
|
rotator.homing_flag = true;
|
|
} else {
|
|
rotator.rotator_status = error;
|
|
rotator.rotator_error = homing_error;
|
|
}
|
|
} else {
|
|
/* Control Loop */
|
|
stepper_az.moveTo(deg2step(control_az.setpoint));
|
|
stepper_el.moveTo(deg2step(control_el.setpoint));
|
|
rotator.rotator_status = pointing;
|
|
stepper_az.run();
|
|
stepper_el.run();
|
|
/* Idle rotator */
|
|
if (stepper_az.distanceToGo() == 0 && stepper_el.distanceToGo() == 0) {
|
|
rotator.rotator_status = idle;
|
|
}
|
|
}
|
|
} else {
|
|
/* error handler */
|
|
stepper_az.stop();
|
|
stepper_az.disableOutputs();
|
|
stepper_el.stop();
|
|
stepper_el.disableOutputs();
|
|
if (rotator.rotator_error != homing_error) {
|
|
/* Reset Error flags */
|
|
rotator.rotator_error = no_error;
|
|
rotator.rotator_status = idle;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*Homing Function*/
|
|
enum _rotator_error homing(int32_t seek_az, int32_t seek_el) {
|
|
bool isHome_az = false;
|
|
bool isHome_el = false;
|
|
|
|
stepper_az.moveTo(seek_az);
|
|
stepper_el.moveTo(seek_el);
|
|
|
|
/* Homing loop */
|
|
while (isHome_az == false || isHome_el == false) {
|
|
/* Update WDT */
|
|
wdt.watchdog_reset();
|
|
/* Homing routine */
|
|
if (switch_az.get_state() == true && !isHome_az) {
|
|
stepper_az.moveTo(stepper_az.currentPosition());
|
|
isHome_az = true;
|
|
}
|
|
if (switch_el.get_state() == true && !isHome_el) {
|
|
stepper_el.moveTo(stepper_el.currentPosition());
|
|
isHome_el = true;
|
|
}
|
|
if ((stepper_az.distanceToGo() == 0 && !isHome_az) ||
|
|
(stepper_el.distanceToGo() == 0 && !isHome_el)){
|
|
return homing_error;
|
|
}
|
|
stepper_az.run();
|
|
stepper_el.run();
|
|
}
|
|
/* Delay to Deccelerate and homing */
|
|
uint32_t time = millis();
|
|
while (millis() - time < HOME_DELAY) {
|
|
wdt.watchdog_reset();
|
|
stepper_az.run();
|
|
stepper_el.run();
|
|
}
|
|
/*Reset the steps*/
|
|
stepper_az.setCurrentPosition(0);
|
|
stepper_el.setCurrentPosition(0);
|
|
control_az.setpoint = 0;
|
|
control_el.setpoint = 0;
|
|
|
|
return no_error;
|
|
}
|
|
|
|
/*Convert degrees to steps*/
|
|
int32_t deg2step(float deg) {
|
|
return (RATIO * SPR * deg / 360);
|
|
}
|
|
|
|
/*Convert steps to degrees*/
|
|
float step2deg(int32_t step) {
|
|
return (360.00 * step / (SPR * RATIO));
|
|
}
|