spacecruft
/
satnogs-rotator-firmware
1
0
Fork 0
Code Issues Pull Requests Releases Activity

Move to shared file common functions 

Update stepper motor controller

Signed-off-by: zisi <agzisim@gmail.com>
merge-requests/5/head
zisi 2018-07-22 21:41:03 +03:00
parent 7923ad3d5d
commit df29781d51
14 changed files with 877 additions and 280 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
libraries/as5601.h 100755
View File

@ -0,0 +1,105 @@
#ifndef AS5601_H_
#define AS5601_H_
#include <Wire.h>
#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(&current_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_ */

366
libraries/easycomm.h 100644
View File

@ -0,0 +1,366 @@
/*
* easycomm.h
*
* Created on: Jul 18, 2018
* Author: azisi
*/
#ifndef LIBRARIES_EASYCOMM_H_
#define LIBRARIES_EASYCOMM_H_
#include <Arduino.h>
#include <WString.h>
#include <avr/wdt.h>
#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_ */

0
stepper_motor_controller/endstop.h → libraries/endstop.h
View File

58
libraries/globals.h 100644
View File

@ -0,0 +1,58 @@
/*
* globals.h
*
* Created on: Jul 18, 2018
* Author: azisi
*/
#ifndef LIBRARIES_GLOBALS_H_
#define LIBRARIES_GLOBALS_H_
#include <Arduino.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,
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_ */

2
stepper_motor_controller/i2c_mux.h → libraries/i2c_mux.h
View File

@ -10,6 +10,8 @@
#include <Wire.h>
#define I2C_FREQ 100000
class i2c_mux {
public:

152
libraries/motor.h 100755
View File

@ -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_ */

36
libraries/rotator_pins.h 100755
View File

@ -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_ */

0
stepper_motor_controller/rs485.h → libraries/rs485.h
View File

0
stepper_motor_controller/tc74.h → libraries/tc74.h
View File

80
libraries/watchdog.h 100755
View File

@ -0,0 +1,80 @@
/*
* watchdog.h
*
* Created on: Nov 12, 2017
* Author: azisi
*/
#ifndef WATCHDOG_H_
#define WATCHDOG_H_
#include <avr/wdt.h>
#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_ */

48
stepper_motor_controller/rotator_config.h
View File

@ -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_ */

266
stepper_motor_controller/stepper_motor_controller.ino
View File

@ -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 <AccelStepper.h>
#include <Wire.h>
#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;
}

25
stepper_motor_controller/watchdog.cpp
View File

@ -1,25 +0,0 @@
/*
* watchdog.c
*
* Created on: Nov 12, 2017
* Author: azisi
*/
#include "watchdog.h"
#include <Arduino.h>
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
}

19
stepper_motor_controller/watchdog.h
View File

@ -1,19 +0,0 @@
/*
* watchdog.h
*
* Created on: Nov 12, 2017
* Author: azisi
*/
#ifndef WATCHDOG_H_
#define WATCHDOG_H_
#include <avr/wdt.h>
void watchdog_init(uint8_t);
inline void watchdog_reset() {
wdt_reset();
}
#endif /* WATCHDOG_H_ */