From df29781d517613d2c8e47182c63d8da96f81e408 Mon Sep 17 00:00:00 2001 From: zisi Date: Sun, 22 Jul 2018 21:41:03 +0300 Subject: [PATCH] Move to shared file common functions Update stepper motor controller Signed-off-by: zisi --- libraries/as5601.h | 105 +++++ libraries/easycomm.h | 366 ++++++++++++++++++ .../endstop.h | 0 libraries/globals.h | 58 +++ .../i2c_mux.h | 2 + libraries/motor.h | 152 ++++++++ libraries/rotator_pins.h | 36 ++ .../rs485.h | 0 .../tc74.h | 0 libraries/watchdog.h | 80 ++++ stepper_motor_controller/rotator_config.h | 48 --- .../stepper_motor_controller.ino | 266 ++++--------- stepper_motor_controller/watchdog.cpp | 25 -- stepper_motor_controller/watchdog.h | 19 - 14 files changed, 877 insertions(+), 280 deletions(-) create mode 100755 libraries/as5601.h create mode 100644 libraries/easycomm.h rename {stepper_motor_controller => libraries}/endstop.h (100%) create mode 100644 libraries/globals.h rename {stepper_motor_controller => libraries}/i2c_mux.h (96%) create mode 100755 libraries/motor.h create mode 100755 libraries/rotator_pins.h rename {stepper_motor_controller => libraries}/rs485.h (100%) rename {stepper_motor_controller => libraries}/tc74.h (100%) create mode 100755 libraries/watchdog.h delete mode 100755 stepper_motor_controller/rotator_config.h delete mode 100755 stepper_motor_controller/watchdog.cpp delete mode 100755 stepper_motor_controller/watchdog.h diff --git a/libraries/as5601.h b/libraries/as5601.h new file mode 100755 index 0000000..244a4ee --- /dev/null +++ b/libraries/as5601.h @@ -0,0 +1,105 @@ +#ifndef AS5601_H_ +#define AS5601_H_ + +#include + +#define I2C_FREQ 100000 + +/* Encoder defines */ +#define AS5601_ID 0x36 +#define RAW_ANG_HIGH 0x0C +#define RAW_ANG_LOW 0x0D +#define STATUS_REG 0x0B +#define AGC 0x1A +#define MAGNITUDE_HIGH 0x1B +#define MAGNITUDE_LOW 0x1C +#define CONF_HIGH 0x07 +#define CONF_LOW 0x08 + +class AS5601 { +public: + void Begin() { + Wire.begin(); + Wire.setClock(I2C_FREQ); + } + + /* Read Encoder */ + uint8_t get_pos(double *new_pos) { + uint16_t raw_angle; + uint8_t status_val; + float raw_pos = 0; + float delta_raw_pos = 0; + float real_pos = 0; + + raw_angle = i2c_word_transaction(AS5601_ID, RAW_ANG_HIGH); + /* Read Status Bits */ + status_val = i2c_byte_transaction(AS5601_ID, STATUS_REG); + /* Check the status register */ + if ((status_val & 0x20) && !(status_val & 0x10) + && !(status_val & 0x08)) { + raw_pos = (float) raw_angle * 0.0879; + delta_raw_pos = _raw_prev_pos - raw_pos; + if (delta_raw_pos > 180) + _n++; + else if (delta_raw_pos < -180) + _n--; + real_pos = ((raw_pos + 360 * _n) / _enc_ratio) - _angle_offset; + _raw_prev_pos = raw_pos; + } else + ; + *new_pos = (double)real_pos; + return status_val; + } + + uint8_t get_agc() { + return i2c_byte_transaction(AS5601_ID, AGC); + } + + uint16_t get_magnitude() { + return i2c_word_transaction(AS5601_ID, MAGNITUDE_HIGH); + } + + uint16_t get_conf() { + return i2c_word_transaction(AS5601_ID, CONF_HIGH); + } + + uint8_t set_zero() { + double current_pos; + uint8_t status_val = get_pos(¤t_pos); + _angle_offset = current_pos; + return status_val; + } + + void init_zero() { + _angle_offset = 0.0; + } + + void set_gear_ratio(uint8_t enc_ratio) { + _enc_ratio = enc_ratio; + } + +private: + double _angle_offset = 0; + int32_t _n = 0; + float _raw_prev_pos = 0; + uint8_t _enc_ratio = 0; + + uint8_t i2c_byte_transaction(uint8_t i2c_address, uint8_t i2c_register) { + Wire.beginTransmission(i2c_address); + Wire.write(i2c_register); + Wire.endTransmission(); + Wire.requestFrom(i2c_address, (uint8_t) 1); + while (Wire.available() == 0) + ; + return Wire.read(); + } + + uint16_t i2c_word_transaction(uint8_t i2c_address,uint8_t i2c_register) { + uint8_t word_high = i2c_byte_transaction(i2c_address, i2c_register); + uint8_t word_low = i2c_byte_transaction(i2c_address, i2c_register + 1); + return ((word_high << 8) | word_low); + } +}; + +#endif /* AS5601_H_ */ + diff --git a/libraries/easycomm.h b/libraries/easycomm.h new file mode 100644 index 0000000..0e6a2e9 --- /dev/null +++ b/libraries/easycomm.h @@ -0,0 +1,366 @@ +/* + * easycomm.h + * + * Created on: Jul 18, 2018 + * Author: azisi + */ + +#ifndef LIBRARIES_EASYCOMM_H_ +#define LIBRARIES_EASYCOMM_H_ + +#include +#include +#include +#include "rs485.h" +#include "rotator_pins.h" +#include "globals.h" + +/* Communication interface */ +#define RS485_TX_TIME 9 // Wait t ms to write in serial +#define BUFFER_SIZE 256 +#define BAUDRATE 19200 + +rs485 rs485(RS485_DIR, RS485_TX_TIME); + +class easycomm { +public: + void easycomm_init() { + rs485.begin(BAUDRATE); + } + + /*EasyComm 3 Protocol & Calculate the steps*/ + void easycomm_proc() { + /*Serial*/ + char buffer[BUFFER_SIZE]; + char incomingByte; + char *Data = buffer; + char *rawData; + static uint16_t BufferCnt = 0; + char data[100]; + String str1, str2, str3, str4, str5, str6; + + /*Read from serial*/ + while (rs485.available() > 0) { + incomingByte = rs485.read(); + + /*new data*/ + if (incomingByte == '\n') { + buffer[BufferCnt] = 0; + if (buffer[0] == 'A' && buffer[1] == 'Z') { + if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L') { + /*Get position*/ + str1 = String("AZ"); + str2 = String(control_az.input, 1); + str3 = String(" EL"); + str4 = String(control_el.input, 1); + str5 = String("\n"); + rs485.print(str1 + str2 + str3 + str4 + str5); + } else { + /*Get the absolute value of angle*/ + rotator.control_mode = position; + rawData = strtok_r(Data, " ", &Data); + strncpy(data, rawData + 2, 10); + if (isNumber(data)) { + /*Calculate the steps*/ + control_az.setpoint = atof(data); + } + /*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)) { + /*Calculate the steps*/ + control_el.setpoint = atof(data); + } + } + } + } else if (buffer[0] == 'V' && buffer[1] == 'U') { + /* Elevation increase speed */ + rotator.control_mode = speed; + strncpy(data, Data + 2, 10); + if (isNumber(data)) { + /*Calculate the steps*/ + control_el.setpoint_speed = atof(data) / 1000; + } + } else if (buffer[0] == 'V' && buffer[1] == 'D') { + /* Elevation decrease speed */ + rotator.control_mode = speed; + strncpy(data, Data + 2, 10); + if (isNumber(data)) { + /*Calculate the steps*/ + control_el.setpoint_speed = - atof(data) / 1000; + } + } else if (buffer[0] == 'V' && buffer[1] == 'L') { + /* Azimuth increase speed */ + rotator.control_mode = speed; + strncpy(data, Data + 2, 10); + if (isNumber(data)) { + /*Calculate the steps*/ + control_az.setpoint_speed = atof(data) / 1000; + } + } else if (buffer[0] == 'V' && buffer[1] == 'R') { + /* Azimuth decrease speed */ + rotator.control_mode = speed; + strncpy(data, Data + 2, 10); + if (isNumber(data)) { + /*Calculate the steps*/ + control_az.setpoint_speed = - atof(data) / 1000; + } + } else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' + && buffer[3] == 'S' && buffer[4] == 'E') { + /* Stop Moving */ + rotator.control_mode = position; + str1 = String("AZ"); + str2 = String(control_az.input, 1); + str3 = String(" EL"); + str4 = String(control_el.input, 1); + str5 = String("\n"); + rs485.print(str1 + str2 + str3 + str4 + str5); + control_az.setpoint = control_az.input; + control_el.setpoint = control_el.input; + } else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' + && buffer[3] == 'E' && buffer[4] == 'T') { + /* Reset the rotator */ + str1 = String("AZ"); + str2 = String(control_az.input, 1); + str3 = String(" EL"); + str4 = String(control_el.input, 1); + str5 = String("\n"); + rs485.print(str1 + str2 + str3 + str4 + str5); + rotator.homing_flag = false; + } else if (buffer[0] == 'P' && buffer[1] == 'A' && buffer[2] == 'R' + && buffer[3] == 'K' ) { + /* Park the rotator */ + str1 = String("AZ"); + str2 = String(control_az.input, 1); + str3 = String(" EL"); + str4 = String(control_el.input, 1); + str5 = String("\n"); + rs485.print(str1 + str2 + str3 + str4 + str5); + control_az.setpoint = rotator.park_az; + control_el.setpoint = rotator.park_el; + } else if (buffer[0] == 'V' && buffer[1] == 'E') { + str1 = String("VE"); + str2 = String("SatNOGS-v2"); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '0') { + str1 = String("IP0,"); + str2 = String(rotator.inside_temperature, DEC); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '1') { + str1 = String("IP1,"); + str2 = String(rotator.switch_az, DEC); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '2') { + str1 = String("IP2,"); + str2 = String(rotator.switch_el, DEC); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '3') { + str1 = String("IP3,"); + str2 = String(control_az.input, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '4') { + str1 = String("IP4,"); + str2 = String(control_el.input, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '5') { + str1 = String("IP5,"); + str2 = String(control_az.load, DEC); // Get load for motor driver + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '6') { + str1 = String("IP6,"); + str2 = String(control_el.load, DEC); // Get load for motor driver + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '7') { + str1 = String("IP7,"); + str2 = String(control_az.speed, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '8') { + str1 = String("IP8,"); + str2 = String(control_el.speed, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'G' && buffer[1] == 'S') { + str1 = String("GS"); + str2 = String(rotator.rotator_status, DEC); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'G' && buffer[1] == 'E') { + str1 = String("GE"); + str2 = String(rotator.rotator_error, DEC); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if(buffer[0] == 'C' && buffer[1] == 'R') { + /* Get Config + * 1 get Kp Azimuth gain + * 2 get Ki Azimuth gain + * 3 get Kd Azimuth gain + * 4 get Kp Elevation gain + * 5 get Ki Elevation gain + * 6 get Kd Elevation gain + * 7 get Azimuth park position + * 8 get Elevation park position + * 9 control mode */ + if (buffer[3] == '1') { + /* Get the Kp gain */ + str1 = String("1,"); + str2 = String(control_az.p, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[3] == '2') { + /* Get the Ki gain */ + str1 = String("2,"); + str2 = String(control_az.i, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[3] == '3') { + /* Get the Kd gain */ + str1 = String("3,"); + str2 = String(control_az.d, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[3] == '4') { + /* Get the Kp gain */ + str1 = String("4,"); + str2 = String(control_el.p, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[3] == '5') { + /* Get the Ki gain */ + str1 = String("5,"); + str2 = String(control_el.i, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[3] == '6') { + /* Get the Kd gain */ + str1 = String("6,"); + str2 = String(control_el.d, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[3] == '7') { + /* Get the Kd gain */ + str1 = String("7,"); + str2 = String(rotator.park_az, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[3] == '8') { + /* Get the Kd gain */ + str1 = String("8,"); + str2 = String(rotator.park_el, 2); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[3] == '9') { + /* Get control mode */ + str1 = String("9,"); + str2 = String(rotator.control_mode); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } + } else if (buffer[0] == 'C' && buffer[1] == 'W') { + /* Set Config + * 1 set Kp Azimuth gain + * 2 set Ki Azimuth gain + * 3 set Kd Azimuth gain + * 4 set Kp Elevation gain + * 5 set Ki Elevation gain + * 6 set Kd Elevation gain + * 7 set Azimuth park position + * 8 set Elevation park position + * 9 control mode is set by cmd Vx + * 10 reset error in EEPROM */ + if (buffer[2] == '1') { + /* Set the Kp gain */ + rawData = strtok_r(Data, ",", &Data); + strncpy(data, rawData + 4, 10); + if (isNumber(data)) { + control_az.p = atof(data); + } + } else if (buffer[2] == '2') { + /* Get the Ki gain */ + rawData = strtok_r(Data, ",", &Data); + strncpy(data, rawData + 4, 10); + if (isNumber(data)) { + control_az.i = atof(data); + } + } else if (buffer[2] == '3') { + /* Set the Kd gain */ + rawData = strtok_r(Data, ",", &Data); + strncpy(data, rawData + 4, 10); + if (isNumber(data)) { + control_az.d = atof(data); + } + } else if (buffer[2] == '4') { + /* Set the Kp gain */ + rawData = strtok_r(Data, ",", &Data); + strncpy(data, rawData + 4, 10); + if (isNumber(data)) { + control_el.p = atof(data); + } + } else if (buffer[2] == '5') { + /* Set the Ki gain */ + rawData = strtok_r(Data, ",", &Data); + strncpy(data, rawData + 4, 10); + if (isNumber(data)) { + control_el.i = atof(data); + } + } else if (buffer[2] == '6') { + /* Set the Kd gain */ + rawData = strtok_r(Data, ",", &Data); + strncpy(data, rawData + 4, 10); + if (isNumber(data)) { + control_el.d = atof(data); + } + } else if (buffer[2] == '7') { + /* Set the Azimuth park position */ + rawData = strtok_r(Data, ",", &Data); + strncpy(data, rawData + 4, 10); + if (isNumber(data)) { + rotator.park_az = atof(data); + } + } else if (buffer[2] == '8') { + /* Set the Elevation park position */ + rawData = strtok_r(Data, ",", &Data); + strncpy(data, rawData + 4, 10); + if (isNumber(data)) { + rotator.park_el = atof(data); + } + } + } else if (buffer[0] == 'R' && buffer[1] == 'S' + && buffer[2] == 'T') { + while(1) + ; + } else if (buffer[0] == 'R' && buffer[1] == 'B') { + wdt_enable(WDTO_2S); + while(1); + } + BufferCnt = 0; + rs485.flush(); + } else { + /*Fill the buffer with incoming data*/ + buffer[BufferCnt] = incomingByte; + BufferCnt++; + } + } + } + +private: + /*Check if is argument in number*/ + bool isNumber(char *input) { + for (uint16_t i = 0; input[i] != '\0'; i++) { + if (isalpha(input[i])) + return false; + } + return true; + } +}; + +#endif /* LIBRARIES_EASYCOMM_H_ */ diff --git a/stepper_motor_controller/endstop.h b/libraries/endstop.h similarity index 100% rename from stepper_motor_controller/endstop.h rename to libraries/endstop.h diff --git a/libraries/globals.h b/libraries/globals.h new file mode 100644 index 0000000..40e93a7 --- /dev/null +++ b/libraries/globals.h @@ -0,0 +1,58 @@ +/* + * globals.h + * + * Created on: Jul 18, 2018 + * Author: azisi + */ + +#ifndef LIBRARIES_GLOBALS_H_ +#define LIBRARIES_GLOBALS_H_ + +#include + +/******************************************************************************/ +enum _rotator_status { + idle = 1, moving = 2, pointing = 4, error = 8 +}; +enum _rotator_error { + no_error = 1, sensor_error = 2, homing_error = 4, motor_error = 8, + over_temperature = 12, wdt_error = 16 +}; +enum _control_mode { + position = 0, speed = 1 +}; +struct _control{ + double input; + double input_prv; + double speed; + double setpoint; + double setpoint_speed; + uint16_t load; + double u; + double p, i, d; +}; +struct _rotator{ + volatile enum _rotator_status rotator_status; + volatile enum _rotator_error rotator_error; + enum _control_mode control_mode; + bool homing_flag; + int8_t inside_temperature; + double park_az, park_el; + uint8_t fault_az, fault_el; + bool switch_az, switch_el; +}; +/******************************************************************************/ +_control control_az = { .input = 0, .input_prv = 0, .speed=0, .setpoint = 0, + .setpoint_speed = 0, .load = 0, .u = 0, .p = 8.0, + .i = 0.0, .d = 0.5 }; +_control control_el = { .input = 0, .input_prv = 0, .speed=0, .setpoint = 0, + .setpoint_speed = 0, .load = 0, .u = 0, .p = 10.0, + .i = 0.0, .d = 0.3 }; +_rotator rotator = { .rotator_status = idle, .rotator_error = no_error, + .control_mode = position, .homing_flag = false, + .inside_temperature = 0, .park_az = 0, .park_el = 0, + .fault_az = LOW, .fault_el = LOW , .switch_az = false, + .switch_el = false}; +/******************************************************************************/ + +#endif /* LIBRARIES_GLOBALS_H_ */ diff --git a/stepper_motor_controller/i2c_mux.h b/libraries/i2c_mux.h similarity index 96% rename from stepper_motor_controller/i2c_mux.h rename to libraries/i2c_mux.h index 52a970a..cc6f050 100755 --- a/stepper_motor_controller/i2c_mux.h +++ b/libraries/i2c_mux.h @@ -10,6 +10,8 @@ #include +#define I2C_FREQ 100000 + class i2c_mux { public: diff --git a/libraries/motor.h b/libraries/motor.h new file mode 100755 index 0000000..c153e5d --- /dev/null +++ b/libraries/motor.h @@ -0,0 +1,152 @@ +#ifndef MOTOR_H_ +#define MOTOR_H_ + +class motor { + +public: + motor(uint8_t pwm_pin1, uint8_t pwm_pin2, uint8_t fb_pin, uint8_t en_pin, + uint8_t sf_pin, uint16_t maxSpeed, uint16_t minSpeed) { + _pwm_pin1 = pwm_pin1; + _pwm_pin2 = pwm_pin2; + _maxSpeed = maxSpeed; + _minSpeed = minSpeed; + _fb_pin = fb_pin; + _en_pin = en_pin; + _sf_pin = sf_pin; + stop(); + } + + void init_pin() { + pinMode(_pwm_pin1, OUTPUT); + pinMode(_pwm_pin2, OUTPUT); + /* Feedback and sense */ + pinMode(_fb_pin, INPUT); + pinMode(_sf_pin,INPUT); + /* Enable Motors */ + pinMode(_en_pin, OUTPUT); + } + + void init_timer(uint8_t timer, uint16_t divisor) { + // Set PWM frequency for D5 & D6 + // set timer 0 divisor to 1 for PWM frequency of 62500.00 Hz + // set timer 0 divisor to 8 for PWM frequency of 7812.50 Hz + // set timer 0 divisor to 64 for PWM frequency of 976.56 Hz (The DEFAULT) + // set timer 0 divisor to 256 for PWM frequency of 244.14 Hz + // set timer 0 divisor to 1024 for PWM frequency of 61.04 Hz + if (timer == 0) { + if (divisor == 1) { + TCCR0B = (TCCR0B & B11111000) | B00000001; + } else if (divisor == 8) { + TCCR0B = (TCCR0B & B11111000) | B00000010; + } else if (divisor == 64) { + TCCR0B = (TCCR0B & B11111000) | B00000011; + } else if (divisor == 256) { + TCCR0B = (TCCR0B & B11111000) | B00000100; + } else if (divisor == 1024) { + TCCR0B = (TCCR0B & B11111000) | B00000101; + } + } + // Set PWM frequency for D9 & D10 + // set timer 1 divisor to 1 for PWM frequency of 31372.55 Hz + // set timer 1 divisor to 8 for PWM frequency of 3921.16 Hz + // set timer 1 divisor to 64 for PWM frequency of 490.20 Hz (The DEFAULT) + // set timer 1 divisor to 256 for PWM frequency of 122.55 Hz + // set timer 1 divisor to 1024 for PWM frequency of 30.64 Hz + if (timer == 1) { + if (divisor == 1) { + TCCR1B = (TCCR1B & B11111000) | B00000001; + } else if (divisor == 8) { + TCCR1B = (TCCR1B & B11111000) | B00000010; + } else if (divisor == 64) { + TCCR1B = (TCCR1B & B11111000) | B00000011; + } else if (divisor == 256) { + TCCR1B = (TCCR1B & B11111000) | B00000100; + } else if (divisor == 1024) { + TCCR1B = (TCCR1B & B11111000) | B00000101; + } + } + // Set PWM frequency for D3 & D11 + // set timer 2 divisor to 1 for PWM frequency of 31372.55 Hz + // set timer 2 divisor to 8 for PWM frequency of 3921.16 Hz + // set timer 2 divisor to 32 for PWM frequency of 980.39 Hz + // set timer 2 divisor to 64 for PWM frequency of 490.20 Hz (The DEFAULT) + // set timer 2 divisor to 128 for PWM frequency of 245.10 Hz + // set timer 2 divisor to 256 for PWM frequency of 122.55 Hz + // set timer 2 divisor to 1024 for PWM frequency of 30.64 Hz + if (timer == 2) { + if (divisor == 1) { + TCCR2B = (TCCR2B & B11111000) | B00000001; + } else if (divisor == 8) { + TCCR2B = (TCCR2B & B11111000) | B00000010; + } else if (divisor == 32){ + TCCR2B = (TCCR2B & B11111000) | B00000011; + } else if (divisor == 64) { + TCCR2B = (TCCR2B & B11111000) | B00000100; + } else if (divisor == 128) { + TCCR2B = (TCCR2B & B11111000) | B00000101; + } else if (divisor == 256) { + TCCR2B = (TCCR2B & B11111000) | B00000110; + } else if (divisor == 1024) { + TCCR2B = (TCCR2B & B11111000) | B00000111; + } + } + } + + void enable() { + digitalWrite(_en_pin, HIGH); + } + + void disenable() { + digitalWrite(_en_pin, LOW); + } + + uint16_t get_load() { + return analogRead(_fb_pin); + } + + uint8_t get_fault() + { + return digitalRead(_sf_pin); + } + + void move(int16_t speed) { + if (speed == 0) { + stop(); + return; + } + + if (speed >= 0) { + speed = speed + _minSpeed; + if (speed > _maxSpeed) + speed = _maxSpeed; + analogWrite(_pwm_pin1, 0); + analogWrite(_pwm_pin2, speed); + } else { + speed = -speed; + speed = speed + _minSpeed; + if (speed > _maxSpeed) + speed = _maxSpeed; + analogWrite(_pwm_pin1, speed); + analogWrite(_pwm_pin2, 0); + } + } + + void stop() { + analogWrite(_pwm_pin1, 0); + analogWrite(_pwm_pin2, 0); + } + + void set_min(uint16_t min) { + _maxSpeed = min; + } + + void set_max(uint16_t max) { + _maxSpeed = max; + } + +private: + uint8_t _pwm_pin1, _pwm_pin2, _fb_pin, _en_pin, _sf_pin; + int16_t _maxSpeed, _minSpeed; +}; + +#endif /* MOTOR_H_ */ diff --git a/libraries/rotator_pins.h b/libraries/rotator_pins.h new file mode 100755 index 0000000..7449122 --- /dev/null +++ b/libraries/rotator_pins.h @@ -0,0 +1,36 @@ +/* + * rotator_features.h + * + * Created on: Nov 12, 2017 + * Author: azisi + */ + +#ifndef ROTATOR_PINS_H_ +#define ROTATOR_PINS_H_ + +#define M1IN1 10 +#define M1IN2 9 +#define M1SF 7 +#define M1FB A1 + +#define M2IN1 11 +#define M2IN2 3 +#define M2SF 6 +#define M2FB A0 + +#define MOTOR_EN 8 + +#define SW1 5 +#define SW2 4 + +#define RS485_DIR 2 + +#define SDA_PIN 3 +#define SCL_PIN 4 + +#define PIN12 12 +#define PIN13 13 +#define A2 A2 +#define A3 A3 + +#endif /* ROTATOR_PINS_H_ */ diff --git a/stepper_motor_controller/rs485.h b/libraries/rs485.h similarity index 100% rename from stepper_motor_controller/rs485.h rename to libraries/rs485.h diff --git a/stepper_motor_controller/tc74.h b/libraries/tc74.h similarity index 100% rename from stepper_motor_controller/tc74.h rename to libraries/tc74.h diff --git a/libraries/watchdog.h b/libraries/watchdog.h new file mode 100755 index 0000000..138a6f1 --- /dev/null +++ b/libraries/watchdog.h @@ -0,0 +1,80 @@ +/* + * watchdog.h + * + * Created on: Nov 12, 2017 + * Author: azisi + */ + +#ifndef WATCHDOG_H_ +#define WATCHDOG_H_ + +#include +#include "globals.h" +#include "easycomm.h" +#include "rotator_pins.h" + +class wdt_timer{ +public: + + void watchdog_init() { + cli(); + wdt_reset(); + WDTCSR |= _BV(WDCE) | _BV(WDE); + WDTCSR = _BV(WDIE) | _BV(WDE) | _BV(WDP3) | _BV(WDP2) | _BV(WDP1); + sei(); + } + + void watchdog_reset() { + wdt_reset(); + } +}; + +ISR(WDT_vect) { + /* Disable motors */ + digitalWrite(MOTOR_EN, LOW); + /* Set error */ + rotator.rotator_error = wdt_error; + rotator.rotator_status = error; + sei(); + + while (1) { + wdt_reset(); + /*Serial*/ + char buffer[BUFFER_SIZE]; + char incomingByte; + static uint16_t BufferCnt = 0; + String str1, str2, str3, str4, str5, str6; + + /*Read from serial*/ + while (rs485.available() > 0) { + incomingByte = rs485.read(); + + /*new data*/ + if (incomingByte == '\n') { + buffer[BufferCnt] = 0; + if (buffer[0] == 'G' && buffer[1] == 'S') { + str1 = String("GS"); + str2 = String(rotator.rotator_status, DEC); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'G' && buffer[1] == 'E') { + str1 = String("GE"); + str2 = String(rotator.rotator_error, DEC); + str3 = String("\n"); + rs485.print(str1 + str2 + str3); + } else if (buffer[0] == 'R' && buffer[1] == 'B') { + while(1); + } + BufferCnt = 0; + rs485.flush(); + } else { + /*Fill the buffer with incoming data*/ + buffer[BufferCnt] = incomingByte; + BufferCnt++; + } + } + wdt_reset(); + } +} + +#endif /* WATCHDOG_H_ */ diff --git a/stepper_motor_controller/rotator_config.h b/stepper_motor_controller/rotator_config.h deleted file mode 100755 index ba2fb0d..0000000 --- a/stepper_motor_controller/rotator_config.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * rotator_config.h - * - * Created on: Mar 3, 2017 - * Author: azisi - */ - -#ifndef ROTATOR_CONFIG_H_ -#define ROTATOR_CONFIG_H_ - -#define SAMPLE_TIME 1000 /* in ms */ -#define WDT_TIMEOUT WDTO_2S -#define RATIO 54 /*Gear ratio*/ - -/* I2C Multiplexer */ -#define I2C_FREQ 100000 -#define PCA9540_ID 0x70 -#define PCA9540_CH0 0x04 -#define PCA9540_CH1 0x05 -/* Temperature Sensor */ -#define TC74_ID 0x4D -/* Encoder AS5601 */ -#define ENC_RATIO 2 - -/* DC Motors */ -#define MAX_PWM 255 -#define MIN_PWM 35 - -/* Stepper Motors */ -#define MIN_PULSE_WIDTH 20 /*in microsecond*/ -#define MAX_SPEED 400 -#define MAX_ACCELERATION 100 -#define SPR 200 /*Step Per Revolution*/ - -/* Homing Functions */ -#define MIN_M1_ANGLE 0 /*Maximum Angle of Azimuth for homing scanning*/ -#define MAX_M1_ANGLE 365 /*Maximum Angle of Azimuth for homing scanning*/ -#define MIN_M2_ANGLE 0 -#define MAX_M2_ANGLE 180 /*Maximum Angle of Elevation for homing scanning*/ -#define DEFAULT_HOME_STATE HIGH /*Change to LOW according to Home sensor*/ -#define HOME_DELAY 10000 /*Time for homing Decceleration in millisecond*/ - -/* Communication interface */ -#define BufferSize 256 -#define BaudRate 19200 -#define RS485_TX_TIME 9 // ms - -#endif /* ROTATOR_CONFIG_H_ */ diff --git a/stepper_motor_controller/stepper_motor_controller.ino b/stepper_motor_controller/stepper_motor_controller.ino index 5a4fc80..4f087cd 100644 --- a/stepper_motor_controller/stepper_motor_controller.ino +++ b/stepper_motor_controller/stepper_motor_controller.ino @@ -1,42 +1,50 @@ +/******************************************************************************/ +/******************************************************************************/ +/******************************************************************************/ +#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 #include -#include "rotator_pins.h" -#include "rotator_config.h" -#include "rs485.h" -#include "endstop.h" -#include "watchdog.h" -#include "i2c_mux.h" -#include "tc74.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" /******************************************************************************/ -enum _rotator_status { - idle = 1, moving = 2, pointing = 4, error = 8 -}; -enum _rotator_error { - no_error = 1, sensor_error = 2, homing_error = 4, motor_error = 8 -}; - +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); -void cmd_proc(int32_t &setpoint_az, int32_t &setpoit_el); int32_t deg2step(float deg); float step2deg(int32_t step); -bool isNumber(char *input); - -/******************************************************************************/ -AccelStepper stepper_az(1, M1IN1, M1IN2), stepper_el(1, M2IN1, M2IN2); -rs485 rs485(RS485_DIR, RS485_TX_TIME); -endstop switch_az(SW1, DEFAULT_HOME_STATE), switch_el(SW2, DEFAULT_HOME_STATE); -i2c_mux pca9540(PCA9540_ID, PCA9540_CH0, PCA9540_CH1); -tc74 temp_sensor(TC74_ID); - -volatile enum _rotator_status rotator_status = idle; -volatile enum _rotator_error rotator_error = no_error; -volatile bool homing_flag = true; -static int32_t step_az = 0, step_el = 0; /******************************************************************************/ 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); @@ -50,48 +58,49 @@ void setup() { stepper_el.setAcceleration(MAX_ACCELERATION); stepper_el.setMinPulseWidth(MIN_PULSE_WIDTH); - /* Homing switch */ - switch_az.init(); - switch_el.init(); - - /* Initialize I2C MUX */ - pca9540.init(); - - /* Serial Communication */ - rs485.begin(BaudRate); - /* Initialize WDT */ - watchdog_init(WDT_TIMEOUT); + wdt.watchdog_init(); } void loop() { /* Update WDT */ - watchdog_reset(); + 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*/ - cmd_proc(step_az, step_el); + comm.easycomm_proc(); - if (rotator_status != error) { - if (homing_flag == false) { - rotator_error = homing(deg2step(-MAX_M1_ANGLE), deg2step(-MAX_M2_ANGLE)); - if (rotator_error == no_error) { + /* 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*/ - step_az = 0; - step_el = 0; - rotator_status = idle; - homing_flag = true; + rotator.rotator_status = idle; + rotator.homing_flag = true; } else { - rotator_status = error; + rotator.rotator_status = error; + rotator.rotator_error = homing_error; } } else { - /*Move the Azimuth & Elevation Motor*/ - stepper_az.moveTo(step_az); - stepper_el.moveTo(step_el); + /* 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(); - rotator_status = pointing; + /* Idle rotator */ if (stepper_az.distanceToGo() == 0 && stepper_el.distanceToGo() == 0) { - rotator_status = idle; + rotator.rotator_status = idle; } } } else { @@ -100,6 +109,11 @@ void loop() { 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; + } } } @@ -114,20 +128,18 @@ enum _rotator_error homing(int32_t seek_az, int32_t seek_el) { /* Homing loop */ while (isHome_az == false || isHome_el == false) { /* Update WDT */ - watchdog_reset(); + wdt.watchdog_reset(); /* Homing routine */ - if (switch_az.get_state() == true) { + if (switch_az.get_state() == true && !isHome_az) { stepper_az.moveTo(stepper_az.currentPosition()); isHome_az = true; } - if (switch_el.get_state() == 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) { - return homing_error; - } - if (stepper_el.distanceToGo() == 0 && !isHome_el) { + if ((stepper_az.distanceToGo() == 0 && !isHome_az) || + (stepper_el.distanceToGo() == 0 && !isHome_el)){ return homing_error; } stepper_az.run(); @@ -136,132 +148,19 @@ enum _rotator_error homing(int32_t seek_az, int32_t seek_el) { /* Delay to Deccelerate and homing */ uint32_t time = millis(); while (millis() - time < HOME_DELAY) { - watchdog_reset(); + 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; } -/*EasyComm 2 Protocol & Calculate the steps*/ -void cmd_proc(int32_t &setpoint_az, int32_t &setpoint_el) { - /*Serial*/ - char buffer[BufferSize]; - char incomingByte; - char *Data = buffer; - char *rawData; - static uint16_t BufferCnt = 0; - char data[100]; - String str1, str2, str3, str4, str5, str6; - - /*Read from serial*/ - while (rs485.available() > 0) { - incomingByte = rs485.read(); - - /*new data*/ - if (incomingByte == '\n') { - buffer[BufferCnt] = 0; - if (buffer[0] == 'A' && buffer[1] == 'Z') { - if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L') { - /*Get position*/ - str1 = String("AZ"); - str2 = String(step2deg(stepper_az.currentPosition()), 1); - str3 = String(" EL"); - str4 = String(step2deg(stepper_el.currentPosition()), 1); - str5 = String("\n"); - rs485.print(str1 + str2 + str3 + str4 + str5); - } else { - /*Get the absolute value of angle*/ - rawData = strtok_r(Data, " ", &Data); - strncpy(data, rawData + 2, 10); - if (isNumber(data)) { - /*Calculate the steps*/ - setpoint_az = deg2step(atof(data)); - } - /*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)) { - /*Calculate the steps*/ - setpoint_el = deg2step(atof(data)); - } - } - } - } - /*Stop Moving*/ - else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' - && buffer[3] == 'S' && buffer[4] == 'E') { - /*Get position*/ - str1 = String("AZ"); - str2 = String(step2deg(stepper_az.currentPosition()), 1); - str3 = String(" EL"); - str4 = String(step2deg(stepper_el.currentPosition()), 1); - str5 = String("\n"); - rs485.print(str1 + str2 + str3 + str4 + str5); - setpoint_az = stepper_az.currentPosition(); - setpoint_el = stepper_el.currentPosition(); - } - /*Reset the rotator*/ - else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' - && buffer[3] == 'E' && buffer[4] == 'T') { - /*Get position*/ - str1 = String("AZ"); - str2 = String(step2deg(stepper_az.currentPosition()), 1); - str3 = String(" EL"); - str4 = String(step2deg(stepper_el.currentPosition()), 1); - str5 = String("\n"); - rs485.print(str1 + str2 + str3 + str4 + str5); - homing_flag = false; - } else if (buffer[0] == 'V' && buffer[1] == 'E') { - str1 = String("VE"); - str2 = String("SatNOGS-v2"); - str3 = String("\n"); - rs485.print(str1 + str2 + str3); - } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '0') { - pca9540.set_channel(PCA9540_CH1); - temp_sensor.wake_up(); - str1 = String("IP0,"); - str2 = String(temp_sensor.get_temp(), DEC); - str3 = String("\n"); - rs485.print(str1 + str2 + str3); - temp_sensor.sleep(); - } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '1') { - str1 = String("IP1,"); - str2 = String(switch_az.get_state(), DEC); - str3 = String("\n"); - rs485.print(str1 + str2 + str3); - } else if (buffer[0] == 'I' && buffer[1] == 'P' && buffer[2] == '2') { - str1 = String("IP2,"); - str2 = String(switch_el.get_state(), DEC); - str3 = String("\n"); - rs485.print(str1 + str2 + str3); - } else if (buffer[0] == 'G' && buffer[1] == 'S') { - str1 = String("GS"); - str2 = String(rotator_status, DEC); - str3 = String("\n"); - rs485.print(str1 + str2 + str3); - } else if (buffer[0] == 'G' && buffer[1] == 'E') { - str1 = String("GE"); - str2 = String(rotator_error, DEC); - str3 = String("\n"); - rs485.print(str1 + str2 + str3); - } - BufferCnt = 0; - rs485.flush(); - } - /*Fill the buffer with incoming data*/ - else { - buffer[BufferCnt] = incomingByte; - BufferCnt++; - } - } -} - /*Convert degrees to steps*/ int32_t deg2step(float deg) { return (RATIO * SPR * deg / 360); @@ -271,12 +170,3 @@ int32_t deg2step(float deg) { float step2deg(int32_t step) { return (360.00 * step / (SPR * RATIO)); } - -/*Check if is argument in number*/ -bool isNumber(char *input) { - for (uint16_t i = 0; input[i] != '\0'; i++) { - if (isalpha(input[i])) - return false; - } - return true; -} diff --git a/stepper_motor_controller/watchdog.cpp b/stepper_motor_controller/watchdog.cpp deleted file mode 100755 index d85514c..0000000 --- a/stepper_motor_controller/watchdog.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/* - * watchdog.c - * - * Created on: Nov 12, 2017 - * Author: azisi - */ - -#include "watchdog.h" -#include - -void watchdog_init(uint8_t delay) { - // We enable the watchdog timer, but only for the interrupt. - // Take care, as this requires the correct order of operation, - // with interrupts disabled. See the datasheet of any AVR chip for details. - - wdt_reset(); - _WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE); - _WD_CONTROL_REG = _BV(WDIE) | delay; -} - -ISR( WDT_vect) { - Serial.println("WDT TIME OUT"); - while (1) - Serial.println("WDT TIME OUT"); //wait for user -} diff --git a/stepper_motor_controller/watchdog.h b/stepper_motor_controller/watchdog.h deleted file mode 100755 index 19461a8..0000000 --- a/stepper_motor_controller/watchdog.h +++ /dev/null @@ -1,19 +0,0 @@ -/* - * watchdog.h - * - * Created on: Nov 12, 2017 - * Author: azisi - */ - -#ifndef WATCHDOG_H_ -#define WATCHDOG_H_ - -#include - -void watchdog_init(uint8_t); - -inline void watchdog_reset() { - wdt_reset(); -} - -#endif /* WATCHDOG_H_ */