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

569 lines
15 KiB
C++
Raw Blame History

#include "SoftI2CMaster.h"
#include <Wire.h>
//#include <avr/wdt.h>
/* 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()
{
/* 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<<WDCE) | (1<<WDE);
//WDTCSR = (0<<WDIE) | (1<<WDE) | (0<<WDP3) | (1<<WDP2) | (1<<WDP1) | (0<<WDP0);
//sei();
/* Initial Homing */
//Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, true);
}
void loop()
{
double *set_point;
double curr_pos[2];
/* Read commands from serial */
set_point = cmd_proc();
//set_point = cmd_proc_RS485();
/* Move Motor */
dc_move(set_point);
/* Reset WDT */
//wdt_reset();
/* Time Check */
if (t_DIS == 0)
t_DIS = millis();
/* Disable Motors */
get_position(curr_pos);
if (abs(curr_pos[0]-set_point[0]) < Deadband && abs(curr_pos[1]-set_point[1]) < Deadband && millis()-t_DIS > T_DELAY)
digitalWrite(EN, LOW);
/* Enable Motors */
else
digitalWrite(EN, HIGH);
}
///* 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];
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)
{
;
}
}
}
}
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
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);
}
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 */
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;
}
Reference in New Issue View Git Blame Copy Permalink