Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
389 lines (355 sloc) 9.93 KB
// Thanks to Satnogs for the easycomm2 code
// This is basically just a hack
// to make their code work with my servo setup
#include <string.h>
#include <stdlib.h>
#include <math.h>
//#include <AccelStepper.h>
#include <Servo.h>
//#define DIR_AZ 6 /*PIN for Azimuth Direction*/
//#define STEP_AZ 5 /*PIN for Azimuth Steps*/
//#define DIR_EL 10 /*PIN for Elevation Direction*/
//#define STEP_EL 9 /*PIN for Elevation Steps*/
//
//#define EN 8 /*PIN for Enable or Disable Stepper Motors*/
//
//#define SPR 200 /*Step Per Revolution*/
//#define RATIO 54 /*Gear ratio*/
//#define T_DELAY 60000 /*Time to disable the motors in millisecond*/
//
//#define HOME_AZ 4 /*Homing switch for Azimuth*/
//#define HOME_EL 7 /*Homing switch for Elevation*/
//
//#define MAX_AZ_ANGLE 365 /*Maximum Angle of Azimuth for homing scanning*/
//#define MAX_EL_ANGLE 365 /*Maximum Angle of Elevation for homing scanning*/
//
//#define MAX_SPEED 300
//#define MAX_ACCELERATION 100
//
//#define MIN_PULSE_WIDTH 20 /*in microsecond*/
//
//#define DEFAULT_HOME_STATE HIGH /*Change to LOW according to Home sensor*/
//
//#define HOME_DELAY 6000 /*Time for homing Decceleration in millisecond*/
#define BufferSize 256
#define BaudRate 19200
// servos
Servo elservo;
Servo roservo;
// servo feedback pins
int elfeedbackPin = 1;
int rofeedbackPin = 0;
/*Global Variables*/
unsigned long t_DIS = 0; /*time to disable the Motors*/
/*Define a stepper and the pins it will use*/
//AccelStepper AZstepper(1, STEP_AZ, DIR_AZ);
//AccelStepper ELstepper(1, STEP_EL, DIR_EL);
void setup()
{
// /*Change these to suit your stepper if you want*/
// AZstepper.setMaxSpeed(MAX_SPEED);
// AZstepper.setAcceleration(MAX_ACCELERATION);
// /*Change these to suit your stepper if you want*/
// ELstepper.setMaxSpeed(MAX_SPEED);
// ELstepper.setAcceleration(MAX_ACCELERATION);
// /*Set minimum pulse width*/
// AZstepper.setMinPulseWidth(MIN_PULSE_WIDTH);
// ELstepper.setMinPulseWidth(MIN_PULSE_WIDTH);
// /*Enable Motors*/
// pinMode(EN, OUTPUT);
// digitalWrite(EN, LOW);
// /*Homing switch*/
// pinMode(HOME_AZ, INPUT_PULLUP);
// pinMode(HOME_EL, INPUT_PULLUP);
// /*Initial Homing*/
// Homing(deg2step(-MAX_AZ_ANGLE), deg2step(-MAX_EL_ANGLE));
/*Serial Communication*/
Serial.begin(BaudRate);
// attach servos
elservo.attach(9);
roservo.attach(8);
}
void loop()
{
/*Define the steps*/
static int AZstep = 0;
static int ELstep = 0;
//static int angleAz = 0;
//static int angleEl = 0;
/*Time Check*/
if (t_DIS == 0)
t_DIS = millis();
// /*Disable Motors*/
// if (AZstep == AZstepper.currentPosition() && ELstep == ELstepper.currentPosition() && millis()-t_DIS > T_DELAY)
// digitalWrite(EN, HIGH);
// /*Enable Motors*/
// else
// digitalWrite(EN, LOW);
/*Read the steps from serial*/
cmd_proc(AZstep, ELstep);
/*Move the Azimuth & Elevation Motor*/
//stepper_move(AZstep, ELstep);
servo_move(AZstep, ELstep);
}
/*Homing Function*/
//void Homing(int AZsteps, int ELsteps)
//{
// int value_Home_AZ = DEFAULT_HOME_STATE;
// int value_Home_EL = DEFAULT_HOME_STATE;
// boolean isHome_AZ = false;
// boolean isHome_EL = false;
//
//// AZstepper.moveTo(AZsteps);
//// ELstepper.moveTo(ELsteps);
//
// while(isHome_AZ == false || isHome_EL == false)
// {
// value_Home_AZ = digitalRead(HOME_AZ);
// value_Home_EL = digitalRead(HOME_EL);
// /*Change to LOW according to Home sensor*/
// if (value_Home_AZ == DEFAULT_HOME_STATE)
// {
//// AZstepper.moveTo(AZstepper.currentPosition());
// isHome_AZ = true;
// }
// /*Change to LOW according to Home sensor*/
// if (value_Home_EL == DEFAULT_HOME_STATE)
// {
//// ELstepper.moveTo(ELstepper.currentPosition());
// isHome_EL = true;
// }
// if (AZstepper.distanceToGo() == 0 && !isHome_AZ)
// {
// error(0);
// break;
// }
// if (ELstepper.distanceToGo() == 0 && !isHome_EL)
// {
// error(1);
// break;
// }
//// AZstepper.run();
//// ELstepper.run();
// }
// /*Delay to Deccelerate*/
// long time = millis();
// while(millis() - time < HOME_DELAY)
// {
//// AZstepper.run();
//// ELstepper.run();
// }
// /*Reset the steps*/
//// AZstepper.setCurrentPosition(0);
//// ELstepper.setCurrentPosition(0);
//}
/*EasyComm 2 Protocol & Calculate the steps*/
void cmd_proc(int &stepAz, int &stepEl)
{
/*Serial*/
char buffer[BufferSize];
char incomingByte;
char *Data = buffer;
char *rawData;
static int BufferCnt = 0;
char data[100];
double angleAz, angleEl;
/*Read from serial*/
while (Serial.available() > 0)
{
incomingByte = Serial.read();
/* XXX: Get position using custom and test code */
if (incomingByte == '!')
{
/*Get position*/
Serial.print("TM");
Serial.print(1);
Serial.print(" ");
Serial.print("AZ");
Serial.print(10*degro(),1);
//Serial.print(10*step2deg(AZstepper.currentPosition()), 1);
Serial.print(" ");
Serial.print("EL");
Serial.print(10*degel(),1);
//Serial.println(10*step2deg(ELstepper.currentPosition()), 1);
}
/*new data*/
else if (incomingByte == '\n')
{
buffer[BufferCnt] = 0;
if (buffer[0] == 'A' && buffer[1] == 'Z')
{
if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L')
{
/*Get position*/
Serial.print("AZ");
Serial.print(10*degro(),1);
//Serial.print(step2deg(AZstepper.currentPosition()), 1);
Serial.print(" ");
Serial.print("EL");
Serial.print(10*degel(),1);
//Serial.print(step2deg(ELstepper.currentPosition()), 1);
Serial.println(" ");
}
else
{
/*Get the absolute value of angle*/
rawData = strtok_r(Data, " " , &Data);
strncpy(data, rawData+2, 10);
if (isNumber(data))
{
angleAz = atof(data);
/*Calculate the steps*/
//stepAz = deg2step(angleAz);
stepAz = angleAz;
}
/*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))
{
angleEl = atof(data);
/*Calculate the steps*/
//stepEl = deg2step(angleEl);
stepEl = angleEl;
}
}
}
}
/*Stop Moving*/
else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E')
{
/*Get position*/
Serial.print("AZ");
// Serial.print(step2deg(AZstepper.currentPosition()), 1);
Serial.print(" ");
Serial.print("EL");
// Serial.println(step2deg(ELstepper.currentPosition()), 1);
// stepAz = AZstepper.currentPosition();
// stepEl = ELstepper.currentPosition();
}
/*Reset the rotator*/
else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T')
{
/*Get position*/
Serial.print("AZ");
// Serial.print(step2deg(AZstepper.currentPosition()), 1);
Serial.print(" ");
Serial.print("EL");
// Serial.println(step2deg(ELstepper.currentPosition()), 1);
/*Move the steppers to initial position*/
// Homing(deg2step(-MAX_AZ_ANGLE), deg2step(-MAX_EL_ANGLE));
/*Zero the steps*/
stepAz = 0;
stepEl = 0;
}
BufferCnt = 0;
/*Reset the disable motor time*/
t_DIS = 0;
}
/*Fill the buffer with incoming data*/
else {
buffer[BufferCnt] = incomingByte;
BufferCnt++;
}
}
}
/*Error Handling*/
void error(int num_error)
{
switch (num_error)
{
/*Azimuth error*/
case (0):
while(1)
{
Serial.println("AL001");
delay(100);
}
/*Elevation error*/
case (1):
while(1)
{
Serial.println("AL002");
delay(100);
}
default:
while(1)
{
Serial.println("AL000");
delay(100);
}
}
}
/*Send pulses to stepper motor drivers*/
void stepper_move(int stepAz, int stepEl)
{
// AZstepper.moveTo(stepAz);
// ELstepper.moveTo(stepEl);
// AZstepper.run();
// ELstepper.run();
}
void servo_move(int stepAz, int stepEl){
int xmape;
int xmapr;
// map the range of angles to the servo
//xmape = map(stepEl, 0, 180, 500, 2400);
xmape = map(stepEl, 0, 90, 2300, 1425);
elservo.writeMicroseconds(xmape);
//xmapr = map(stepAz, 0, 180, 500, 2400);
// translate azimuth angles
// make 0 degrees pointing EAST
//Serial.println(stepAz);
if ((stepAz >= 0) && (stepAz <= 90)){
//quadrant 1
xmapr = map(stepAz, 0, 90, 2300, 1500);
roservo.writeMicroseconds(xmapr);
}
else if((stepAz >= 91) && (stepAz <= 180)){
// quadrant 4
xmapr = map(stepAz, 90, 180, 1500, 700);
roservo.writeMicroseconds(xmapr);
}
}
// convert servo position to degree
int degro(){
int position;
int degree;
// rotation feedback
position = analogRead(rofeedbackPin);
// my servos have analog positions between 80 and 445
// where 445 is 0 deg and 80 is 180 deg
degree = map(position, 445, 125, 0, 180);
return degree;
}
int degel(){
int position;
int degree;
// elevation feedback
position = analogRead(elfeedbackPin);
// my servo reads 285 at 90 degrees
// which is 0 degrees of elevation
degree = map(position, 264, 425, 90, 0);
return degree;
}
///*Convert degrees to steps*/
//int deg2step(double deg)
//{
// return(RATIO*SPR*deg/360);
//}
///*Convert steps to degrees*/
//double step2deg(int Step)
//{
// return(360.00*Step/(SPR*RATIO));
//}
/*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;
}