diff --git a/dc_motor_controller/Makefile b/dc_motor_controller/Makefile new file mode 100755 index 0000000..7d8ee13 --- /dev/null +++ b/dc_motor_controller/Makefile @@ -0,0 +1,26 @@ +# Arduino Make file. Refer to https://github.com/sudar/Arduino-Makefile + +ARDUINO_DIR = /home/azisi/opt/arduino-1.8.5 +ARDMK_DIR = /home/azisi/opt/Arduino-Makefile +AVR_TOOLS_DIR = /home/azisi/opt/arduino-1.8.5/hardware/tools/avr + +ARDUINO_QUIET = 0 + +ARCHITECTURE = avr +BOARD_TAG = satnogs +MONITOR_PORT = /dev/ttyUSB0 + +CFLAGS_STD = -std=gnu++11 +USER_LIB_PATH = /home/azisi/workspace/arduino/libraries + +AVRDUDE = /home/azisi/opt/arduino-1.8.5/hardware/tools/avr/bin/avrdude +AVRDUDE_OPTS = -v + +BOOTLOADER_FILE = optiboot/optiboot_atmega328.hex + +ISP_PROG = buspirate +ISP_PORT = /dev/ttyUSB0 + +#AVRDUDE_ARD_BAUDRATE = 57600 + +include /home/azisi/opt/Arduino-Makefile/Arduino.mk diff --git a/dc_motor_controller/dc_motor_controller.ino b/dc_motor_controller/dc_motor_controller.ino index c3b8723..7becb90 100644 --- a/dc_motor_controller/dc_motor_controller.ino +++ b/dc_motor_controller/dc_motor_controller.ino @@ -1,568 +1,236 @@ -#include "SoftI2CMaster.h" +/******************************************************************************/ +/******************************************************************************/ +/******************************************************************************/ +#define SAMPLE_TIME 0.1 /* Control loop in s */ +#define RATIO 54 /* Gear ratio */ +#define MAX_PWM 180 /* Set max Speed */ +#define MIN_PWM 5 /* Set min Speed */ +#define POSITION_DEADZONE 0.2 /* Control dead zone */ +#define PCA9540_ID 0x70 /* I2C Multiplexer ID */ +#define PCA9540_CH0 0x04 /* I2C Multiplexer CHO */ +#define PCA9540_CH1 0x05 /* I2C Multiplexer CH1 */ +#define TC74_ID 0x48 /* Temperature Sensor ID */ +#define OVER_TEMP 60 /* Over temperature limit */ +#define ENC_RATIO 2 /* Encoder AS5601 gear ratio */ +#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_SPEED 100 /* Set speed to find home, duty cycle 255->100% */ +#define HOME_DELAY 1000 /* Time for homing Deceleration in millisecond */ +/******************************************************************************/ +/******************************************************************************/ +/******************************************************************************/ +#include #include -//#include +#include "../libraries/globals.h" +#include "../libraries/easycomm.h" +#include "../libraries/rotator_pins.h" +#include "../libraries/endstop.h" +#include "../libraries/watchdog.h" +#include "../libraries/i2c_mux.h" +#include "../libraries/tc74.h" +#include "../libraries/motor.h" +#include "../libraries/as5601.h" +/******************************************************************************/ +uint32_t t_run = 0; +easycomm comm; +motor motor_az(M1IN1, M1IN2, M1FB, MOTOR_EN, M1SF, MAX_PWM, MIN_PWM); +motor motor_el(M2IN1, M2IN2, M2FB, MOTOR_EN, M2SF, MAX_PWM, MIN_PWM); +PID pid_az(&control_az.input, &control_az.u, &control_az.setpoint, control_az.p, + control_az.i, control_az.d, P_ON_E, DIRECT); +PID pid_el(&control_el.input, &control_el.u, &control_el.setpoint, control_el.p, + control_el.i, control_el.d, P_ON_E, DIRECT); +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); +AS5601 encoder_az, encoder_el; +wdt_timer wdt; +/******************************************************************************/ +enum _rotator_error homing(); +/******************************************************************************/ -/* H-bridge defines */ -#define PWM1M1 5 -#define PWM2M1 6 -#define PWM1M2 9 -#define PWM2M2 10 -/* Limits for control signal */ -#define outMax 120 -#define outMin 35 -#define Deadband 1 -/* Encoder defines */ -#define sda_pin 16 -#define scl_pin 14 -#define as5601_adr 0x36 -#define raw_ang_high 0x0c -#define raw_ang_low 0x0d -#define status_reg 0x0b -/* Ratio of worm gear */ -#define RATIO 30 -/* Maximum Angle for homing scanning */ -#define SEEK_MOVE 30 -#define MIN_AZ_ANGLE 0 -#define MAX_AZ_ANGLE 370 -#define MIN_EL_ANGLE 0 -#define MAX_EL_ANGLE 110 -/* Homing switch */ -#define HOME_AZ 4 -#define HOME_EL 7 -/* Change to LOW according to Home sensor */ -#define DEFAULT_HOME_STATE 1 -/* Time to disable the motors in millisecond */ -#define T_DELAY 1000 -/* PIN for Enable or Disable Stepper Motors */ -#define EN 8 -/* Serial configuration */ -#define BufferSize 256 -#define BaudRate 19200 -#define TX_EN A3 -/*Global Variables*/ -unsigned long t_DIS = 0; /*time to disable the Motors*/ -/* angle offset */ -double az_angle_offset = 0; -double el_angle_offset = 0; -/* Encoder */ -SoftI2CMaster i2c_soft = SoftI2CMaster(scl_pin, sda_pin, 1); +void setup() { + /* Homing switch */ + switch_az.init(); + switch_el.init(); -void setup() -{ - /* H-bridge */ - pinMode(OUTPUT, PWM1M1); - pinMode(OUTPUT, PWM2M1); - pinMode(OUTPUT, PWM1M2); - pinMode(OUTPUT, PWM2M2); - /* Encoder */ - i2c_soft.begin(); - Wire.begin(); - /* Enable Motors */ - pinMode(EN, OUTPUT); - digitalWrite(EN, HIGH); - /* Homing switch */ - pinMode(HOME_AZ, INPUT_PULLUP); - pinMode(HOME_EL, INPUT_PULLUP); - /* Serial Communication */ - Serial.begin(BaudRate); - /* RS485 */ - //Serial1.begin(BaudRate); - pinMode(TX_EN, OUTPUT); - digitalWrite(TX_EN, LOW); - /* WDT */ - //cli(); - //WDTCSR |= (1< T_DELAY) - digitalWrite(EN, LOW); - /* Enable Motors */ - else - digitalWrite(EN, HIGH); + /* 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 Motor driver status */ + rotator.fault_az = motor_az.get_fault(); + rotator.fault_el = motor_el.get_fault(); + if (rotator.fault_az == LOW || rotator.fault_el == LOW) { + rotator.rotator_status = error; + rotator.rotator_error = motor_error; + } + + /* Get Temperature */ + pca9540.set_channel(PCA9540_CH1); + temp_sensor.wake_up(); + rotator.inside_temperature = temp_sensor.get_temp(); + temp_sensor.sleep(); + if (rotator.inside_temperature > OVER_TEMP) { + rotator.rotator_status = error; + rotator.rotator_error = over_temperature; + } + /* Get encoders */ + pca9540.set_channel(PCA9540_CH0); + encoder_az.get_pos(&control_az.input); + pca9540.set_channel(PCA9540_CH1); + encoder_el.get_pos(&control_el.input); + + /* Check rotator status */ + if (rotator.rotator_status != error) { + if (rotator.homing_flag == false) { + /* Homing */ + rotator.control_mode = position; + rotator.rotator_error = homing(); + if (rotator.rotator_error == no_error) { + rotator.rotator_status = idle; + rotator.homing_flag = true; + } else { + rotator.rotator_status = error; + rotator.rotator_error = homing_error; + } + } else { + /* Control Loop */ + if (millis() - t_run > SAMPLE_TIME * 1000) { + /* Update control gains */ + pid_az.SetTunings(control_az.p, control_az.i, control_az.d); + pid_el.SetTunings(control_el.p, control_el.i, control_el.d); + /* Move the Azimuth & Elevation Motor */ + if (rotator.control_mode == speed) { + control_az.setpoint += control_az.setpoint_speed + * SAMPLE_TIME; + control_el.setpoint += control_el.setpoint_speed + * SAMPLE_TIME; + rotator.rotator_status = moving; + } else { + rotator.rotator_status = pointing; + } + pid_az.Compute(); + motor_az.move(control_az.u); + pid_el.Compute(); + motor_el.move(control_el.u); + control_az.speed = (control_az.input - control_az.input_prv) + / SAMPLE_TIME; + control_az.input_prv = control_az.input; + control_el.speed = (control_el.input - control_el.input_prv) + / SAMPLE_TIME; + control_el.input_prv = control_el.input; + t_run = millis(); + /* Idle rotator, dead-band */ + if ((abs(control_az.setpoint - control_az.input) <= + POSITION_DEADZONE || (control_az.speed == 0)) && + (abs(control_el.setpoint - control_el.input) <= + POSITION_DEADZONE || (control_el.speed == 0))) { + rotator.rotator_status = idle; + } + } + } + } else { + /* error handler */ + motor_az.stop(); + motor_az.disenable(); + motor_el.stop(); + motor_el.disenable(); + if (rotator.rotator_error != homing_error) { + /* Reset Error flags */ + rotator.rotator_error = no_error; + rotator.rotator_status = idle; + } + } } -///* EasyComm 2 Protocol for RS485 */ -//double * cmd_proc_RS485() -//{ -// static double set_point[] = {0, 0}; -// /* Serial */ -// static char buffer[BufferSize]; -// char incomingByte; -// char *Data = buffer; -// char *rawData; -// static int BufferCnt = 0; -// char data[100]; -// double pos[2]; -// -// /* Read from serial */ -// while (Serial1.available() > 0) -// { -// incomingByte = Serial1.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(pos); -// /* Get position */ -// digitalWrite(TX_EN, HIGH); -// delay(8); -// Serial1.print("AZ"); -// Serial1.print(pos[0], 1); -// Serial1.print(" "); -// Serial1.print("EL"); -// Serial1.println(pos[1], 1); -// delay(8); -// digitalWrite(TX_EN, LOW); -// } -// else -// { -// /* Get the absolute value of angle */ -// rawData = strtok_r(Data, " " , &Data); -// strncpy(data, rawData + 2, 10); -// if (isNumber(data)) -// { -// set_point[0] = atof(data); -// if (set_point[0] > MAX_AZ_ANGLE) -// set_point[0] = MAX_AZ_ANGLE; -// else if (set_point[0] < MIN_AZ_ANGLE) -// set_point[0] = MIN_AZ_ANGLE; -// } -// /* 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)) -// { -// set_point[1] = atof(data); -// if (set_point[1] > MAX_EL_ANGLE) -// set_point[1] = MAX_EL_ANGLE; -// else if (set_point[1] < MIN_EL_ANGLE) -// set_point[1] = MIN_EL_ANGLE; -// } -// } -// } -// } -// /* Stop Moving */ -// else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E') -// { -// get_position(pos); -// /* Get position */ -// digitalWrite(TX_EN, HIGH); -// delay(8); -// Serial1.print("AZ"); -// Serial1.print(pos[0], 1); -// Serial1.print(" "); -// Serial1.print("EL"); -// Serial1.println(pos[1], 1); -// delay(8); -// digitalWrite(TX_EN, LOW); -// set_point[0] = pos[0]; -// set_point[1] = pos[1]; -// } -// /* Reset the rotator */ -// else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T') -// { -// get_position(pos); -// /* Get position */ -// digitalWrite(TX_EN, HIGH); -// delay(8); -// Serial1.print("AZ"); -// Serial1.print(pos[0], 1); -// Serial1.print(" "); -// Serial1.print("EL"); -// Serial1.println(pos[1], 1); -// delay(8); -// digitalWrite(TX_EN, LOW); -// /* Move to initial position */ -// Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, false); -// set_point[0] = 0; -// set_point[1] = 0; -// } -// /* Clear serial buffer */ -// BufferCnt = 0; -// Serial1.read(); -// /* Reset the disable motor time */ -// t_DIS = 0; -// } -// /* Fill the buffer with incoming data */ -// else { -// buffer[BufferCnt] = incomingByte; -// BufferCnt++; -// } -// } -// return set_point; -//} - /* Homing Function */ -void Homing(double AZangle, double ELangle, bool Init) -{ - int value_Home_AZ = DEFAULT_HOME_STATE; - int value_Home_EL = DEFAULT_HOME_STATE; - boolean isHome_AZ = false; - boolean isHome_EL = false; - double zero_angle[2]; - double curr_angle[2]; - double set_point[2]; +enum _rotator_error homing() { + bool isHome_az = false; + bool isHome_el = false; - get_position(zero_angle); - if (Init == true) - { - set_point[0] = zero_angle[0]+SEEK_MOVE; - set_point[1] = zero_angle[1]+SEEK_MOVE; - } - else - { - set_point[0] = 10;//zero_angle[0]-MAX_AZ_ANGLE; - set_point[1] = 10;//zero_angle[1]-MAX_EL_ANGLE; - } - while (isHome_AZ == false || isHome_EL == false) - { - dc_move(set_point); - delay(1); - value_Home_AZ = digitalRead(HOME_AZ); - delay(1); - value_Home_EL = digitalRead(HOME_EL); - /* Change to LOW according to Home sensor */ - if (value_Home_AZ == DEFAULT_HOME_STATE && isHome_AZ == false) - { - isHome_AZ = true; - get_position(curr_angle); - set_point[0] = 0; - if (Init) - az_angle_offset = curr_angle[0]; - } - if (value_Home_EL == DEFAULT_HOME_STATE && isHome_EL == false) - { - isHome_EL = true; - get_position(curr_angle); - set_point[1] = 0; - if (Init) - el_angle_offset = curr_angle[1]; - } - get_position(curr_angle); - if (abs(curr_angle[0] - zero_angle[0]) >= SEEK_MOVE && !isHome_AZ && Init) - set_point[0] = -MAX_AZ_ANGLE; - if (abs(curr_angle[1] - zero_angle[1]) >= SEEK_MOVE && !isHome_EL && Init) - set_point[1] = -MAX_EL_ANGLE; - if ((abs(curr_angle[0] - zero_angle[0]) >= MAX_AZ_ANGLE && !isHome_AZ) || (abs(curr_angle[1] - zero_angle[1]) >= MAX_EL_ANGLE && !isHome_EL)) - { - /* set error */ - while(1) - { - ; - } - } - } -} + /* Resest position */ + pca9540.set_channel(PCA9540_CH0); + encoder_az.set_zero(); + pca9540.set_channel(PCA9540_CH1); + encoder_el.set_zero(); -void dc_move(double set_point[]) -{ - double u[2]; - double curr_pos[2]; - static double prev_pos[] = {0, 0}; - double error[2]; - double Iterm[2]; - double Pterm[2]; - double Dterm[2]; - double Kp = 200; - double Ki = 2; - double Kd = 0; - double dt = 0.001; // calculate + /* Move motors with ~constant speed */ + motor_az.move(-HOME_SPEED); + motor_el.move(-HOME_SPEED); - get_position(curr_pos); - error[0] = set_point[0] - curr_pos[0]; - error[1] = set_point[1] - curr_pos[1]; - - Pterm[0] = Kp * error[0]; - Pterm[1] = Kp * error[1]; - - Iterm[0] += Ki * error[0] * dt; - Iterm[1] += Ki * error[1] * dt; - if (Iterm[0] > outMax) Iterm[0] = outMax; - else if (Iterm[0] < outMin) Iterm[0] = outMin; - if (Iterm[1] > outMax) Iterm[1] = outMax; - else if (Iterm[1] < outMin) Iterm[1] = outMin; - - Dterm[0] = Kd * (curr_pos[0] - prev_pos[0]) / dt; - prev_pos[0] = curr_pos[0]; - Dterm[1] = Kd * (curr_pos[1] - prev_pos[1]) / dt; - prev_pos[1] = curr_pos[1]; - - u[0] = Pterm[0] + Iterm[0] + Dterm[0]; - u[1] = Pterm[1] + Iterm[1] + Dterm[1]; - - if (u[0] >= 0) - { - if (u[0] > outMax) - u[0] = outMax; - analogWrite(PWM1M1, u[0]); - analogWrite(PWM2M1, 0); - } - else - { - u[0] = -u[0]; - if ( u[0] > outMax) - u[0] = outMax; - analogWrite(PWM1M1, 0); - analogWrite(PWM2M1, u[0]); - } - - if (u[1] >= 0) - { - if (u[1] > outMax) - u[1] = outMax; - analogWrite(PWM1M2, u[1]); - analogWrite(PWM2M2, 0); - } - else - { - u[1] = -u[1]; - if ( u[1] > outMax) - u[1] = outMax; - analogWrite(PWM1M2, 0); - analogWrite(PWM2M2, u[1]); - } -} - -/* Read Encoders */ -void get_position(double *new_pos) -{ - int low_raw, high_raw, status_val; - double raw_pos = 0; - double delta_raw_pos = 0; - static double raw_prev_pos[] = {0, 0}; - static double real_pos[] = {0, 0}; - static int n[] = {0, 0}; - - /* Axis 1*/ - /* Read Raw Angle Low Byte */ - Wire.beginTransmission(as5601_adr); - Wire.write(raw_ang_low); - Wire.endTransmission(); - Wire.requestFrom(as5601_adr, 1); - while(Wire.available() == 0); - low_raw = Wire.read(); - /* Read Raw Angle High Byte */ - Wire.beginTransmission(as5601_adr); - Wire.write(raw_ang_high); - Wire.endTransmission(); - Wire.requestFrom(as5601_adr, 1); - while(Wire.available() == 0); - high_raw = Wire.read(); - high_raw = high_raw << 8; - high_raw = high_raw | low_raw; - /* Read Status Bits */ - Wire.beginTransmission(as5601_adr); - Wire.write(status_reg); - Wire.endTransmission(); - Wire.requestFrom(as5601_adr, 1); - while(Wire.available() == 0); - status_val = Wire.read(); - /* Check the status register */ - if ((status_val & 0x20) && !(status_val & 0x10) && !(status_val & 0x08)) - { - if (high_raw >= 0) - { - raw_pos = (double)high_raw*0.0879; - delta_raw_pos = raw_prev_pos[0] - raw_pos; - if (delta_raw_pos > 180) - n[0]++; - else if (delta_raw_pos < -180) - n[0]--; - real_pos[0] = ((raw_pos + 360 * n[0]) / RATIO) - az_angle_offset; - raw_prev_pos[0] = raw_pos; - } - else - ; /* set error */ - } - else - ; /* set error */ - - /* Axis 2 */ - /* Read Raw Angle Low Byte */ - i2c_soft.beginTransmission(as5601_adr); - i2c_soft.write(raw_ang_low); - i2c_soft.endTransmission(); - i2c_soft.requestFrom(as5601_adr); - low_raw = i2c_soft.readLast(); - i2c_soft.endTransmission(); - /* Read Raw Angle High Byte */ - i2c_soft.beginTransmission(as5601_adr); - i2c_soft.write(raw_ang_high); - i2c_soft.endTransmission(); - i2c_soft.requestFrom(as5601_adr); - high_raw = i2c_soft.readLast(); - i2c_soft.endTransmission(); - high_raw = high_raw << 8; - high_raw = high_raw | low_raw; - /* Read Status Bits */ - i2c_soft.beginTransmission(as5601_adr); - i2c_soft.write(status_reg); - i2c_soft.endTransmission(); - i2c_soft.requestFrom(as5601_adr); - status_val = i2c_soft.readLast(); - i2c_soft.endTransmission(); - /* Check the status register */ - if ((status_val & 0x20) && !(status_val & 0x10) && !(status_val & 0x08)) - { - if (high_raw >= 0) - { - raw_pos = (double)high_raw*0.0879; - delta_raw_pos = raw_prev_pos[1] - raw_pos; - if (delta_raw_pos > 180) - n[1]++; - else if (delta_raw_pos < -180) - n[1]--; - real_pos[1] = ((raw_pos + 360 * n[1]) / RATIO) - el_angle_offset; - raw_prev_pos[1] = raw_pos; - } - else - ; /* set error */ - } - else - ; /* set error */ - - new_pos[0] = real_pos[0]; - new_pos[1] = real_pos[1]; -} - -/* EasyComm 2 Protocol */ -double * cmd_proc() -{ - static double set_point[] = {0, 0}; - /* Serial */ - char buffer[BufferSize]; - char incomingByte; - char *Data = buffer; - char *rawData; - static int BufferCnt = 0; - char data[100]; - double pos[2]; - - /* Read from serial */ - while (Serial.available() > 0) - { - incomingByte = Serial.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(pos); - /* Get position */ - Serial.print("AZ"); - Serial.print(pos[0], 1); - Serial.print(" "); - Serial.print("EL"); - Serial.println(pos[1], 1); + /* Homing loop */ + while (isHome_az == false || isHome_el == false) { + /* Update WDT */ + wdt.watchdog_reset(); + if (switch_az.get_state() == true && !isHome_az) { + /* Find azimuth home */ + motor_az.stop(); + isHome_az = true; } - else - { - /* Get the absolute value of angle */ - rawData = strtok_r(Data, " " , &Data); - strncpy(data, rawData + 2, 10); - if (isNumber(data)) - { - set_point[0] = atof(data); - if (set_point[0] > MAX_AZ_ANGLE) - set_point[0] = MAX_AZ_ANGLE; - else if (set_point[0] < MIN_AZ_ANGLE) - set_point[0] = MIN_AZ_ANGLE; - } - /* 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)) - { - set_point[1] = atof(data); - if (set_point[1] > MAX_EL_ANGLE) - set_point[1] = MAX_EL_ANGLE; - else if (set_point[1] < MIN_EL_ANGLE) - set_point[1] = MIN_EL_ANGLE; - } - } + if (switch_el.get_state() == true && !isHome_el) { + /* Find elevation home */ + motor_el.stop(); + isHome_el = true; + } + /* Get current position */ + pca9540.set_channel(PCA9540_CH0); + encoder_az.get_pos(&control_az.input); + pca9540.set_channel(PCA9540_CH1); + encoder_el.get_pos(&control_el.input); + if ((abs(control_az.input) > MAX_M1_ANGLE && !isHome_az) + || (abs(control_el.input) > MAX_M2_ANGLE && !isHome_el)) { + return homing_error; } - } - /* Stop Moving */ - else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E') - { - get_position(pos); - /* Get position */ - Serial.print("AZ"); - Serial.print(pos[0], 1); - Serial.print(" "); - Serial.print("EL"); - Serial.println(pos[1], 1); - set_point[0] = pos[0]; - set_point[1] = pos[1]; - } - /* Reset the rotator */ - else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T') - { - get_position(pos); - /* Get position */ - Serial.print("AZ"); - Serial.print(pos[0], 1); - Serial.print(" "); - Serial.print("EL"); - Serial.println(pos[1], 1); - /* Move to initial position */ - Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, false); - set_point[0] = 0; - set_point[1] = 0; - } - BufferCnt = 0; - /* Reset the disable motor time */ - t_DIS = 0; } - /* Fill the buffer with incoming data */ - else { - buffer[BufferCnt] = incomingByte; - BufferCnt++; - } - } - return set_point; -} -/* 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; -} + /* Resest position */ + pca9540.set_channel(PCA9540_CH0); + encoder_az.init_zero(); + encoder_az.set_zero(); + control_az.setpoint = 0; + pca9540.set_channel(PCA9540_CH1); + encoder_el.init_zero(); + encoder_el.set_zero(); + control_el.setpoint = 0; + return no_error; +}