Update firmware for the DC motor controller
Signed-off-by: zisi <agzisim@gmail.com>merge-requests/5/head
parent
b815d733ad
commit
0e74b598b6
|
@ -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
|
|
@ -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 <PID_v1.h>
|
||||||
#include <Wire.h>
|
#include <Wire.h>
|
||||||
//#include <avr/wdt.h>
|
#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 */
|
void setup() {
|
||||||
#define PWM1M1 5
|
/* Homing switch */
|
||||||
#define PWM2M1 6
|
switch_az.init();
|
||||||
#define PWM1M2 9
|
switch_el.init();
|
||||||
#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()
|
/* Serial Communication */
|
||||||
{
|
comm.easycomm_init();
|
||||||
/* H-bridge */
|
|
||||||
pinMode(OUTPUT, PWM1M1);
|
/* Initialize DC motors */
|
||||||
pinMode(OUTPUT, PWM2M1);
|
motor_az.init_pin();
|
||||||
pinMode(OUTPUT, PWM1M2);
|
motor_az.init_timer(1, 8);
|
||||||
pinMode(OUTPUT, PWM2M2);
|
motor_az.enable();
|
||||||
/* Encoder */
|
motor_el.init_pin();
|
||||||
i2c_soft.begin();
|
motor_el.init_timer(2, 8);
|
||||||
Wire.begin();
|
motor_el.enable();
|
||||||
/* Enable Motors */
|
|
||||||
pinMode(EN, OUTPUT);
|
/* Initialize I2C MUX */
|
||||||
digitalWrite(EN, HIGH);
|
pca9540.init();
|
||||||
/* Homing switch */
|
/* Initialize rotary encoders */
|
||||||
pinMode(HOME_AZ, INPUT_PULLUP);
|
encoder_az.set_gear_ratio(ENC_RATIO);
|
||||||
pinMode(HOME_EL, INPUT_PULLUP);
|
encoder_el.set_gear_ratio(ENC_RATIO);
|
||||||
/* Serial Communication */
|
|
||||||
Serial.begin(BaudRate);
|
/* Initialize control parameters */
|
||||||
/* RS485 */
|
pid_az.SetSampleTime(SAMPLE_TIME); // in seconds
|
||||||
//Serial1.begin(BaudRate);
|
pid_az.SetOutputLimits(-MAX_PWM, MAX_PWM );
|
||||||
pinMode(TX_EN, OUTPUT);
|
pid_az.SetMode(AUTOMATIC);
|
||||||
digitalWrite(TX_EN, LOW);
|
pid_el.SetSampleTime(SAMPLE_TIME); // in seconds
|
||||||
/* WDT */
|
pid_el.SetOutputLimits(-MAX_PWM, MAX_PWM);
|
||||||
//cli();
|
pid_el.SetMode(AUTOMATIC);
|
||||||
//WDTCSR |= (1<<WDCE) | (1<<WDE);
|
|
||||||
//WDTCSR = (0<<WDIE) | (1<<WDE) | (0<<WDP3) | (1<<WDP2) | (1<<WDP1) | (0<<WDP0);
|
/* Initialize WDT */
|
||||||
//sei();
|
wdt.watchdog_init();
|
||||||
/* Initial Homing */
|
|
||||||
//Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, true);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop()
|
void loop() {
|
||||||
{
|
/* Update WDT */
|
||||||
double *set_point;
|
wdt.watchdog_reset();
|
||||||
double curr_pos[2];
|
|
||||||
|
|
||||||
/* Read commands from serial */
|
/* Get end stop status */
|
||||||
set_point = cmd_proc();
|
rotator.switch_az = switch_az.get_state();
|
||||||
//set_point = cmd_proc_RS485();
|
rotator.switch_el = switch_el.get_state();
|
||||||
/* Move Motor */
|
|
||||||
dc_move(set_point);
|
/*Read the steps from serial*/
|
||||||
/* Reset WDT */
|
comm.easycomm_proc();
|
||||||
//wdt_reset();
|
|
||||||
/* Time Check */
|
/* Get Motor driver status */
|
||||||
if (t_DIS == 0)
|
rotator.fault_az = motor_az.get_fault();
|
||||||
t_DIS = millis();
|
rotator.fault_el = motor_el.get_fault();
|
||||||
/* Disable Motors */
|
if (rotator.fault_az == LOW || rotator.fault_el == LOW) {
|
||||||
get_position(curr_pos);
|
rotator.rotator_status = error;
|
||||||
if (abs(curr_pos[0]-set_point[0]) < Deadband && abs(curr_pos[1]-set_point[1]) < Deadband && millis()-t_DIS > T_DELAY)
|
rotator.rotator_error = motor_error;
|
||||||
digitalWrite(EN, LOW);
|
}
|
||||||
/* Enable Motors */
|
|
||||||
else
|
/* Get Temperature */
|
||||||
digitalWrite(EN, HIGH);
|
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 */
|
/* Homing Function */
|
||||||
void Homing(double AZangle, double ELangle, bool Init)
|
enum _rotator_error homing() {
|
||||||
{
|
bool isHome_az = false;
|
||||||
int value_Home_AZ = DEFAULT_HOME_STATE;
|
bool isHome_el = false;
|
||||||
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];
|
|
||||||
|
|
||||||
get_position(zero_angle);
|
/* Resest position */
|
||||||
if (Init == true)
|
pca9540.set_channel(PCA9540_CH0);
|
||||||
{
|
encoder_az.set_zero();
|
||||||
set_point[0] = zero_angle[0]+SEEK_MOVE;
|
pca9540.set_channel(PCA9540_CH1);
|
||||||
set_point[1] = zero_angle[1]+SEEK_MOVE;
|
encoder_el.set_zero();
|
||||||
}
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void dc_move(double set_point[])
|
/* Move motors with ~constant speed */
|
||||||
{
|
motor_az.move(-HOME_SPEED);
|
||||||
double u[2];
|
motor_el.move(-HOME_SPEED);
|
||||||
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
|
|
||||||
|
|
||||||
get_position(curr_pos);
|
/* Homing loop */
|
||||||
error[0] = set_point[0] - curr_pos[0];
|
while (isHome_az == false || isHome_el == false) {
|
||||||
error[1] = set_point[1] - curr_pos[1];
|
/* Update WDT */
|
||||||
|
wdt.watchdog_reset();
|
||||||
Pterm[0] = Kp * error[0];
|
if (switch_az.get_state() == true && !isHome_az) {
|
||||||
Pterm[1] = Kp * error[1];
|
/* Find azimuth home */
|
||||||
|
motor_az.stop();
|
||||||
Iterm[0] += Ki * error[0] * dt;
|
isHome_az = true;
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
else
|
if (switch_el.get_state() == true && !isHome_el) {
|
||||||
{
|
/* Find elevation home */
|
||||||
/* Get the absolute value of angle */
|
motor_el.stop();
|
||||||
rawData = strtok_r(Data, " " , &Data);
|
isHome_el = true;
|
||||||
strncpy(data, rawData + 2, 10);
|
}
|
||||||
if (isNumber(data))
|
/* Get current position */
|
||||||
{
|
pca9540.set_channel(PCA9540_CH0);
|
||||||
set_point[0] = atof(data);
|
encoder_az.get_pos(&control_az.input);
|
||||||
if (set_point[0] > MAX_AZ_ANGLE)
|
pca9540.set_channel(PCA9540_CH1);
|
||||||
set_point[0] = MAX_AZ_ANGLE;
|
encoder_el.get_pos(&control_el.input);
|
||||||
else if (set_point[0] < MIN_AZ_ANGLE)
|
if ((abs(control_az.input) > MAX_M1_ANGLE && !isHome_az)
|
||||||
set_point[0] = MIN_AZ_ANGLE;
|
|| (abs(control_el.input) > MAX_M2_ANGLE && !isHome_el)) {
|
||||||
}
|
return homing_error;
|
||||||
/* 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 */
|
|
||||||
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 */
|
/* Resest position */
|
||||||
boolean isNumber(char *input)
|
pca9540.set_channel(PCA9540_CH0);
|
||||||
{
|
encoder_az.init_zero();
|
||||||
for (int i = 0; input[i] != '\0'; i++)
|
encoder_az.set_zero();
|
||||||
{
|
control_az.setpoint = 0;
|
||||||
if (isalpha(input[i]))
|
pca9540.set_channel(PCA9540_CH1);
|
||||||
return false;
|
encoder_el.init_zero();
|
||||||
}
|
encoder_el.set_zero();
|
||||||
return true;
|
control_el.setpoint = 0;
|
||||||
}
|
|
||||||
|
|
||||||
|
return no_error;
|
||||||
|
}
|
||||||
|
|
Loading…
Reference in New Issue