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

Updates in DC motor controller. 

Unwrap the position sensor.
Updates in pid controller.
Works for AZ axis.
merge-requests/29/head
Agis Zisimatos 2015-12-10 20:52:46 +02:00
parent d05aea1372
commit fba3d03674
1 changed files with 155 additions and 185 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

340
Arduino/dc_motor_controller/dc_motor_controller.ino
View File

@ -1,206 +1,139 @@
#define IO1 5 /* H-bridge defines */
#define PWM1 6 #define PWM1M1 5
#define SENPIN 3 #define PWM2M1 6
/* Limits for control signal */
#define outMax 255
#define outMin 0
/* Encoder defines */
#define SELECT_PIN 7 #define SELECT_PIN 7
#define CLOCK_PIN 8 #define CLOCK_PIN 8
#define DATA_PIN 9 #define DATA_PIN 9
/* Ratio of worm gear */
#define RATIO 30 #define RATIO 30
/* Maximum Angle for homing scanning */
#define outMax 255 #define MAX_AZ_ANGLE 370
#define outMin 0 /* Homing switch */
#define HOME_AZ 2
#define MAX_AZ_ANGLE 365 /*Maximum Angle of Azimuth for homing scanning*/ /* Change to LOW according to Home sensor */
#define MAX_EL_ANGLE 365 /*Maximum Angle of Elevation for homing scanning*/ #define DEFAULT_HOME_STATE HIGH
/* Time to disable the motors in millisecond */
#define HOME_AZ 2 /*Homing switch for Azimuth*/ #define T_DELAY 60000
#define HOME_EL 1 /*Homing switch for Elevation*/ /* PIN for Enable or Disable Stepper Motors */
#define DEFAULT_HOME_STATE HIGH /*Change to LOW according to Home sensor*/ #define EN 4
/* Serial configuration */
#define T_DELAY 60000 /*Time to disable the motors in millisecond*/
#define EN 8 /*PIN for Enable or Disable Stepper Motors*/
#define BufferSize 256 #define BufferSize 256
#define BaudRate 19200 #define BaudRate 19200
/*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 */
double az_angle_offset = 0;
void setup() void setup()
{ {
pinMode(OUTPUT, IO1); /* H-bridge */
pinMode(OUTPUT, PWM1); pinMode(OUTPUT, PWM1M1);
pinMode(INPUT, SENPIN); pinMode(OUTPUT, PWM2M1);
/* Encoder */
pinMode(DATA_PIN, INPUT); pinMode(DATA_PIN, INPUT);
pinMode(CLOCK_PIN, OUTPUT); pinMode(CLOCK_PIN, OUTPUT);
pinMode(SELECT_PIN, OUTPUT); pinMode(SELECT_PIN, OUTPUT);
digitalWrite(CLOCK_PIN, HIGH); digitalWrite(CLOCK_PIN, HIGH);
digitalWrite(SELECT_PIN, HIGH); digitalWrite(SELECT_PIN, HIGH);
/* Homing switch */
/*Homing switch*/
pinMode(HOME_AZ, INPUT_PULLUP); pinMode(HOME_AZ, INPUT_PULLUP);
pinMode(HOME_EL, INPUT_PULLUP); /* Serial Communication */
/*Serial Communication*/
Serial.begin(BaudRate); Serial.begin(BaudRate);
/*Initial Homing*/ /* Initial Homing */
//Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE); Homing(-MAX_AZ_ANGLE, true);
/*Enable Motors*/ /* Enable Motors */
pinMode(EN, OUTPUT); pinMode(EN, OUTPUT);
digitalWrite(EN, LOW); digitalWrite(EN, HIGH);
} }
void loop() void loop()
{ {
/*Define the steps*/ static double AZangle = 0;
static double AZangle = 10;
static double ELangle = 10;
/*Time Check*/
if (t_DIS == 0)
t_DIS = millis();
/*Disable Motors*/ /* Read commands from serial */
if ( AZangle == ((float)readPosition() / 1024.0) * 360 && ELangle == ((float)readPosition() / 1024.0) * 360 && millis()-t_DIS > T_DELAY) cmd_proc(AZangle);
digitalWrite(EN, HIGH); /* Move Motor */
/*Enable Motors*/ dc_move(AZangle);
else
digitalWrite(EN, LOW);
/*Read the steps from serial*/
cmd_proc(AZangle, ELangle);
/*Move the Azimuth & Elevation Motor*/
dc_move(AZangle, ELangle);
} }
void dc_move(double az_angle, double el_angle) /* EasyComm 2 Protocol */
{ void cmd_proc(double &angleAz)
static double u = 0;
static double angle = 0;
static double error = 0;
static double Iterm = 0;
static double Pterm = 0;
double Kp = 1800; // 6000
double Ki = 1; //1
double dt = 0.001;
angle = ((float)readPosition() / 1024.0) * 360;
error = az_angle - angle;
//Serial.println(error);
Pterm = Kp*error;
Iterm += (Ki*error)*dt;
if(Iterm > outMax) Iterm= outMax;
else if(Iterm < outMin) Iterm= outMin;
u = map(Pterm+Iterm, -10000, 10000, outMin, outMax);
if (u > 0)
{
analogWrite(IO1, u);
analogWrite(PWM1, 0);
}
else if (u < 0)
{
analogWrite(IO1, 0);
analogWrite(PWM1, u);
}
}
/*EasyComm 2 Protocol & Calculate the steps*/
void cmd_proc(double &angleAz, double &angleEl)
{ {
/*Serial*/ /* Serial */
char buffer[BufferSize]; 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 */
/*Read from serial*/
while (Serial.available() > 0) while (Serial.available() > 0)
{ {
incomingByte = Serial.read(); incomingByte = Serial.read();
/* XXX: Get position using custom and test code */ /* New data */
if (incomingByte == '!') if (incomingByte == '\n')
{
/*Get position*/
Serial.print("TM");
Serial.print(1);
Serial.print(" ");
Serial.print("AZ");
Serial.print(10*((float)readPosition() / 1024.0) * 360, 1);
Serial.print(" ");
Serial.print("EL");
Serial.println(10*((float)readPosition() / 1024.0) * 360, 1);
}
/*new data*/
else if (incomingByte == '\n')
{ {
buffer[BufferCnt] = 0; buffer[BufferCnt] = 0;
if (buffer[0] == 'A' && buffer[1] == 'Z') if (buffer[0] == 'A' && buffer[1] == 'Z')
{ {
if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L') if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L')
{ {
/*Get position*/ /* Get position */
Serial.print("AZ"); Serial.print("AZ");
Serial.print(((float)readPosition() / 1024.0) * 360, 1); Serial.print(getposition(), 1);
Serial.print(" "); Serial.print(" ");
Serial.print("EL"); Serial.print("EL");
Serial.print(((float)readPosition() / 1024.0) * 360, 1); Serial.println(0, 1);
Serial.println(" "); Serial.println(az_angle_offset);
} }
else else
{ {
/*Get the absolute value of angle*/ /* Get the absolute value of angle */
rawData = strtok_r(Data, " " , &Data); rawData = strtok_r(Data, " " , &Data);
strncpy(data, rawData+2, 10); strncpy(data, rawData+2, 10);
if (isNumber(data)) if (isNumber(data))
angleAz = atof(data); angleAz = 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') if (rawData[0] == 'E' && rawData[1] == 'L')
{ {
strncpy(data, rawData+2, 10); strncpy(data, rawData+2, 10);
if (isNumber(data)) if (isNumber(data))
angleEl = atof(data); ;
} }
} }
} }
/*Stop Moving*/ /* Stop Moving */
else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E') else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E')
{ {
/*Get position*/ angleAz = getposition();
/* Get position */
Serial.print("AZ"); Serial.print("AZ");
Serial.print(((float)readPosition() / 1024.0) * 360, 1); Serial.print(getposition(), 1);
Serial.print(" "); Serial.print(" ");
Serial.print("EL"); Serial.print("EL");
Serial.println(((float)readPosition() / 1024.0) * 360, 1); Serial.println(0, 1);
angleAz = ((float)readPosition() / 1024.0) * 360;
angleEl = ((float)readPosition() / 1024.0) * 360;
} }
/*Reset the rotator*/ /* Reset the rotator */
else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T') else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T')
{ {
/*Get position*/ /* Get position */
Serial.print("AZ"); Serial.print("AZ");
Serial.print(((float)readPosition() / 1024.0) * 360, 1); Serial.print(getposition(), 1);
Serial.print(" "); Serial.print(" ");
Serial.print("EL"); Serial.print("EL");
Serial.println(((float)readPosition() / 1024.0) * 360, 1); Serial.println(0, 1);
/*Move the steppers to initial position*/ /* Move to initial position */
Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE); Homing(-MAX_AZ_ANGLE, false);
/*Zero the steps*/
angleAz = 0; angleAz = 0;
angleEl = 0; }
}
BufferCnt = 0; BufferCnt = 0;
/*Reset the disable motor time*/
t_DIS = 0;
} }
/*Fill the buffer with incoming data*/ /* Fill the buffer with incoming data */
else { else {
buffer[BufferCnt] = incomingByte; buffer[BufferCnt] = incomingByte;
BufferCnt++; BufferCnt++;
@ -208,72 +141,106 @@ void cmd_proc(double &angleAz, double &angleEl)
} }
} }
/*Homing Function*/ /* Homing Function */
void Homing(double AZangle, double ELangle) void Homing(double AZangle, bool Init)
{ {
int value_Home_AZ = DEFAULT_HOME_STATE; int value_Home_AZ = DEFAULT_HOME_STATE;
int value_Home_EL = DEFAULT_HOME_STATE;
boolean isHome_AZ = false; boolean isHome_AZ = false;
boolean isHome_EL = false; double az_zero_angle = getposition();
while(isHome_AZ == false )// || isHome_EL == false)
{
dc_move(AZangle, ELangle);
while(isHome_AZ == false )
{
dc_move(AZangle);
value_Home_AZ = digitalRead(HOME_AZ); value_Home_AZ = digitalRead(HOME_AZ);
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 = true; isHome_AZ = true;
} if (Init)
/*Change to LOW according to Home sensor*/ az_angle_offset = getposition();
if (value_Home_EL == DEFAULT_HOME_STATE)
{
isHome_EL = true;
} }
if (((float)readPosition() / 1024.0) * 360 == 0 && !isHome_AZ) if (abs(getposition() - az_zero_angle) > MAX_AZ_ANGLE && !isHome_AZ)
{ {
error(0); /* set error */
break;
}
if (((float)readPosition() / 1024.0) * 360 == 0 && !isHome_EL)
{
error(1);
break; break;
} }
} }
} }
/*Error Handling*/ void dc_move(double set_point_az)
void error(int num_error)
{ {
switch (num_error) static double u = 0;
static double angle_az = 0;
static double prev_angle_az = 0;
static double error = 0;
static double Iterm = 0;
static double Pterm = 0;
static double Dterm = 0;
double Kp = 2000;
double Ki = 1;
double Kd = 0;
double dt = 0.001;
angle_az = getposition();
error = set_point_az - angle_az;
Pterm = Kp*error;
Iterm += Ki*error*dt;
if(Iterm > outMax) Iterm= outMax;
else if(Iterm < outMin) Iterm= outMin;
Dterm = Kd*(angle_az - prev_angle_az)/dt;
prev_angle_az = angle_az;
u = Pterm+Iterm+Dterm;
if (u >= 0)
{ {
/*Azimuth error*/ if (u > outMax)
case (0): u = outMax;
while(1) analogWrite(PWM1M1, u);
{ analogWrite(PWM2M1, 0);
Serial.println("AL001"); }
delay(100); else
} {
/*Elevation error*/ u = -u;
case (1): if ( u > outMax)
while(1) u = outMax;
{ analogWrite(PWM1M1, 0);
Serial.println("AL002"); analogWrite(PWM2M1, u);
delay(100);
}
default:
while(1)
{
Serial.println("AL000");
delay(100);
}
} }
} }
/*Check if is argument in number*/ /* get position from encoder */
double getposition()
{
double encoder_pos;
double delta_pos;
double real_pos;
static double prev_pos = 0;
int raw_pos;
static int n = 0;
raw_pos = readPosition();
if (raw_pos >= 0)
{
encoder_pos = ((float)raw_pos / 1024.0) * 360.0;
delta_pos = prev_pos - encoder_pos;
if (delta_pos > 180)
n++;
else if (delta_pos < -180)
n--;
real_pos = ((encoder_pos + 360*n)/RATIO) - az_angle_offset;
prev_pos = encoder_pos;
}
else
; /* set error */
return real_pos;
}
/* check if is argument in number */
boolean isNumber(char *input) boolean isNumber(char *input)
{ {
for (int i = 0; input[i] != '\0'; i++) for (int i = 0; input[i] != '\0'; i++)
@ -284,45 +251,47 @@ boolean isNumber(char *input)
return true; return true;
} }
//read the current angular position /* Code from http://reprap.org/wiki/Magnetic_Rotary_Encoder_1.0 */
/* read the current angular position */
int readPosition() int readPosition()
{ {
unsigned int position = 0; unsigned int position = 0;
//shift in our data /* shift in our data */
digitalWrite(SELECT_PIN, LOW); digitalWrite(SELECT_PIN, LOW);
delayMicroseconds(0.5); delayMicroseconds(0.5);
byte d1 = shiftIn(DATA_PIN, CLOCK_PIN); byte d1 = shiftIn(DATA_PIN, CLOCK_PIN);
byte d2 = shiftIn(DATA_PIN, CLOCK_PIN); byte d2 = shiftIn(DATA_PIN, CLOCK_PIN);
digitalWrite(SELECT_PIN, HIGH); digitalWrite(SELECT_PIN, HIGH);
//get our position variable /* get our position variable */
position = d1; position = d1;
position = position << 8; position = position << 8;
position |= d2; position |= d2;
position = position >> 6; position = position >> 6;
//check the offset compensation flag: 1 == started up /* check the offset compensation flag: 1 == started up */
if (!(d2 & B00100000)) if (!(d2 & B00100000))
position = -1; position = -1;
//check the cordic overflow flag: 1 = error /* check the cordic overflow flag: 1 = error */
if (d2 & B00010000) if (d2 & B00010000)
position = -2; position = -2;
//check the linearity alarm: 1 = error /* check the linearity alarm: 1 = error */
if (d2 & B00001000) if (d2 & B00001000)
position = -3; position = -3;
//check the magnet range: 11 = error /* check the magnet range: 11 = error */
if ((d2 & B00000110) == B00000110) if ((d2 & B00000110) == B00000110)
position = -4; position = -4;
return position; return position;
} }
//read in a byte of data from the digital input of the board. /* read in a byte of data from the digital input of the board. */
byte shiftIn(byte data_pin, byte clock_pin) byte shiftIn(byte data_pin, byte clock_pin)
{ {
byte data = 0; byte data = 0;
@ -339,6 +308,7 @@ byte shiftIn(byte data_pin, byte clock_pin)
data |= (bit << i); data |= (bit << i);
} }
return data; return data;
} }