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

Fixes in DC motor firmware. 

Changes in homing function.
Changes in get position function.
Add RS485 serial communication.
merge-requests/29/head
zisi 2016-04-08 17:10:38 +03:00
parent e0857abfdd
commit 8120a43696
1 changed files with 297 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

430
firmware/dc_motor_controller/dc_motor_controller.ino
View File

@ -1,5 +1,6 @@
#include "SoftI2CMaster.h" #include "SoftI2CMaster.h"
#include <Wire.h> #include <Wire.h>
//#include <avr/wdt.h>
/* H-bridge defines */ /* H-bridge defines */
#define PWM1M1 5 #define PWM1M1 5
@ -7,8 +8,9 @@
#define PWM1M2 9 #define PWM1M2 9
#define PWM2M2 10 #define PWM2M2 10
/* Limits for control signal */ /* Limits for control signal */
#define outMax 127 #define outMax 120
#define outMin 0 #define outMin 35
#define Deadband 1
/* Encoder defines */ /* Encoder defines */
#define sda_pin 16 #define sda_pin 16
#define scl_pin 14 #define scl_pin 14
@ -19,20 +21,24 @@
/* Ratio of worm gear */ /* Ratio of worm gear */
#define RATIO 30 #define RATIO 30
/* Maximum Angle for homing scanning */ /* Maximum Angle for homing scanning */
#define MAX_AZ_ANGLE 110 #define SEEK_MOVE 30
#define MAX_EL_ANGLE 370 #define MIN_AZ_ANGLE 0
#define MAX_AZ_ANGLE 370
#define MIN_EL_ANGLE 0
#define MAX_EL_ANGLE 110
/* Homing switch */ /* Homing switch */
#define HOME_AZ 4 #define HOME_AZ 4
#define HOME_EL 7 #define HOME_EL 7
/* Change to LOW according to Home sensor */ /* Change to LOW according to Home sensor */
#define DEFAULT_HOME_STATE 1 #define DEFAULT_HOME_STATE 1
/* Time to disable the motors in millisecond */ /* Time to disable the motors in millisecond */
#define T_DELAY 60000 #define T_DELAY 1000
/* PIN for Enable or Disable Stepper Motors */ /* PIN for Enable or Disable Stepper Motors */
#define EN 8 #define EN 8
/* Serial configuration */ /* Serial configuration */
#define BufferSize 256 #define BufferSize 256
#define BaudRate 19200 #define BaudRate 19200
#define TX_EN A3
/*Global Variables*/ /*Global Variables*/
unsigned long t_DIS = 0; /*time to disable the Motors*/ unsigned long t_DIS = 0; /*time to disable the Motors*/
/* angle offset */ /* angle offset */
@ -51,120 +57,167 @@ void setup()
/* Encoder */ /* Encoder */
i2c_soft.begin(); i2c_soft.begin();
Wire.begin(); Wire.begin();
/* Enable Motors */
pinMode(EN, OUTPUT);
digitalWrite(EN, HIGH);
/* Homing switch */ /* Homing switch */
pinMode(HOME_AZ, INPUT_PULLUP); pinMode(HOME_AZ, INPUT_PULLUP);
pinMode(HOME_EL, INPUT_PULLUP); pinMode(HOME_EL, INPUT_PULLUP);
/* Serial Communication */ /* Serial Communication */
Serial.begin(BaudRate); Serial.begin(BaudRate);
/* Enable Motors */
pinMode(EN, OUTPUT);
digitalWrite(EN, HIGH);
/* RS485 */ /* RS485 */
//Serial1.begin(9600); //Serial1.begin(BaudRate);
//pinMode(A3, OUTPUT); pinMode(TX_EN, OUTPUT);
//digitalWrite(A3, HIGH); 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 */ /* Initial Homing */
Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, true); //Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, true);
} }
void loop() void loop()
{ {
double *set_point; double *set_point;
double curr_pos[2];
/* Read commands from serial */ /* Read commands from serial */
set_point = cmd_proc(); set_point = cmd_proc();
//set_point = cmd_proc_RS485();
/* Move Motor */ /* Move Motor */
dc_move(set_point); 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 */ ///* EasyComm 2 Protocol for RS485 */
double * cmd_proc() //double * cmd_proc_RS485()
{ //{
static double set_point[] = {0, 0}; // static double set_point[] = {0, 0};
/* Serial */ // /* Serial */
char buffer[BufferSize]; // static char buffer[BufferSize];
char incomingByte; // char incomingByte;
char *Data = buffer; // char *Data = buffer;
char *rawData; // char *rawData;
static int BufferCnt = 0; // static int BufferCnt = 0;
char data[100]; // char data[100];
/* Read from serial */ // double pos[2];
while (Serial.available() > 0) //
{ // /* Read from serial */
incomingByte = Serial.read(); // while (Serial1.available() > 0)
/* New data */ // {
if (incomingByte == '\n') // incomingByte = Serial1.read();
{ // /* New data */
buffer[BufferCnt] = 0; // if (incomingByte == '\n')
if (buffer[0] == 'A' && buffer[1] == 'Z') // {
{ // buffer[BufferCnt] = 0;
if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L') // if (buffer[0] == 'A' && buffer[1] == 'Z')
{ // {
double *pos; // if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L')
pos = get_position(); // {
/* Get position */ // get_position(pos);
Serial.print("AZ"); // /* Get position */
Serial.print(pos[0], 1); // digitalWrite(TX_EN, HIGH);
Serial.print(" "); // delay(8);
Serial.print("EL"); // Serial1.print("AZ");
Serial.println(pos[1], 1); // Serial1.print(pos[0], 1);
} // Serial1.print(" ");
else // Serial1.print("EL");
{ // Serial1.println(pos[1], 1);
/* Get the absolute value of angle */ // delay(8);
rawData = strtok_r(Data, " " , &Data); // digitalWrite(TX_EN, LOW);
strncpy(data, rawData + 2, 10); // }
if (isNumber(data)) // else
set_point[0] = atof(data); // {
/* Get the absolute value of angle */ // /* Get the absolute value of angle */
rawData = strtok_r(Data, " " , &Data); // rawData = strtok_r(Data, " " , &Data);
if (rawData[0] == 'E' && rawData[1] == 'L') // strncpy(data, rawData + 2, 10);
{ // if (isNumber(data))
strncpy(data, rawData + 2, 10); // {
if (isNumber(data)) // set_point[0] = atof(data);
set_point[1] = 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;
/* Stop Moving */ // }
else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E') // /* Get the absolute value of angle */
{ // rawData = strtok_r(Data, " " , &Data);
double *pos; // if (rawData[0] == 'E' && rawData[1] == 'L')
pos = get_position(); // {
/* Get position */ // strncpy(data, rawData + 2, 10);
Serial.print("AZ"); // if (isNumber(data))
Serial.print(pos[0], 1); // {
Serial.print(" "); // set_point[1] = atof(data);
Serial.print("EL"); // if (set_point[1] > MAX_EL_ANGLE)
Serial.println(pos[1], 1); // set_point[1] = MAX_EL_ANGLE;
set_point[0] = pos[0]; // else if (set_point[1] < MIN_EL_ANGLE)
set_point[1] = pos[1]; // set_point[1] = MIN_EL_ANGLE;
} // }
/* Reset the rotator */ // }
else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T') // }
{ // }
double *pos; // /* Stop Moving */
pos = get_position(); // else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E')
/* Get position */ // {
Serial.print("AZ"); // get_position(pos);
Serial.print(pos[0], 1); // /* Get position */
Serial.print(" "); // digitalWrite(TX_EN, HIGH);
Serial.print("EL"); // delay(8);
Serial.println(pos[1], 1); // Serial1.print("AZ");
/* Move to initial position */ // Serial1.print(pos[0], 1);
Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, false); // Serial1.print(" ");
set_point[0] = 0; // Serial1.print("EL");
set_point[1] = 0; // Serial1.println(pos[1], 1);
} // delay(8);
BufferCnt = 0; // digitalWrite(TX_EN, LOW);
} // set_point[0] = pos[0];
/* Fill the buffer with incoming data */ // set_point[1] = pos[1];
else { // }
buffer[BufferCnt] = incomingByte; // /* Reset the rotator */
BufferCnt++; // else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T')
} // {
} // get_position(pos);
return set_point; // /* 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) void Homing(double AZangle, double ELangle, bool Init)
@ -173,69 +226,76 @@ void Homing(double AZangle, double ELangle, bool Init)
int value_Home_EL = DEFAULT_HOME_STATE; int value_Home_EL = DEFAULT_HOME_STATE;
boolean isHome_AZ = false; boolean isHome_AZ = false;
boolean isHome_EL = false; boolean isHome_EL = false;
double *zero_angle; double zero_angle[2];
double *curr_angle; double curr_angle[2];
double set_point[] = {0, 0}; double set_point[2];
zero_angle = get_position(); get_position(zero_angle);
if (Init == true)
if (zero_angle[0] > 0) {
set_point[0] = AZangle; set_point[0] = zero_angle[0]+SEEK_MOVE;
set_point[1] = zero_angle[1]+SEEK_MOVE;
}
else else
set_point[0] = -AZangle; {
set_point[0] = 10;//zero_angle[0]-MAX_AZ_ANGLE;
if (zero_angle[1] > 0) set_point[1] = 10;//zero_angle[1]-MAX_EL_ANGLE;
set_point[1] = ELangle; }
else
set_point[1] = -ELangle;
while (isHome_AZ == false || isHome_EL == false) while (isHome_AZ == false || isHome_EL == false)
{ {
dc_move(set_point); dc_move(set_point);
delay(1);
value_Home_AZ = digitalRead(HOME_AZ); value_Home_AZ = digitalRead(HOME_AZ);
delay(1);
value_Home_EL = digitalRead(HOME_EL); value_Home_EL = digitalRead(HOME_EL);
/* Change to LOW according to Home sensor */ /* Change to LOW according to Home sensor */
if (value_Home_AZ == DEFAULT_HOME_STATE) if (value_Home_AZ == DEFAULT_HOME_STATE && isHome_AZ == false)
{ {
isHome_AZ = true; isHome_AZ = true;
curr_angle = get_position(); get_position(curr_angle);
set_point[0] = 0; set_point[0] = 0;
if (Init) if (Init)
az_angle_offset = curr_angle[0]; az_angle_offset = curr_angle[0];
} }
if (value_Home_EL == DEFAULT_HOME_STATE) if (value_Home_EL == DEFAULT_HOME_STATE && isHome_EL == false)
{ {
isHome_EL = true; isHome_EL = true;
curr_angle = get_position(); get_position(curr_angle);
set_point[1] = 0; set_point[1] = 0;
if (Init) if (Init)
el_angle_offset = curr_angle[1]; el_angle_offset = curr_angle[1];
} }
curr_angle = get_position(); get_position(curr_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)) 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 */ /* set error */
break; while(1)
{
;
}
} }
} }
} }
void dc_move(double set_point[]) void dc_move(double set_point[])
{ {
static double u[] = {0, 0}; double u[2];
static double *curr_pos; double curr_pos[2];
static double prev_pos[] = {0, 0}; static double prev_pos[] = {0, 0};
static double error[] = {0, 0}; double error[2];
static double Iterm[] = {0, 0}; double Iterm[2];
static double Pterm[] = {0, 0}; double Pterm[2];
static double Dterm[] = {0, 0}; double Dterm[2];
double Kp = 200; double Kp = 200;
double Ki = 1; double Ki = 2;
double Kd = 0; double Kd = 0;
double dt = 0.001; // calculate double dt = 0.001; // calculate
curr_pos = get_position(); get_position(curr_pos);
error[0] = set_point[0] - curr_pos[0]; error[0] = set_point[0] - curr_pos[0];
error[1] = set_point[1] - curr_pos[1]; error[1] = set_point[1] - curr_pos[1];
@ -291,7 +351,7 @@ void dc_move(double set_point[])
} }
/* Read Encoders */ /* Read Encoders */
double * get_position() void get_position(double *new_pos)
{ {
int low_raw, high_raw, status_val; int low_raw, high_raw, status_val;
double raw_pos = 0; double raw_pos = 0;
@ -388,7 +448,111 @@ double * get_position()
else else
; /* set error */ ; /* set error */
return real_pos; 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 */ /* check if is argument in number */