kopia lustrzana https://github.com/ozarchie/EQMOD-ETX
Initial commit
ozarchie
GitHub
rodzic
f43011aeb5
commit
5ac06ecce8
|
|
@ -0,0 +1,150 @@
|
|||
/*
|
||||
* Copyright 2017, 2018 John Archbold
|
||||
*/
|
||||
|
||||
/********************************************************
|
||||
EQG2HBX program definitions
|
||||
===========================
|
||||
*********************************************************/
|
||||
|
||||
#ifndef EQG2HBX
|
||||
#define EQG2HBX
|
||||
|
||||
// Real Time Clock Libraries
|
||||
// Time related libararies
|
||||
#include <DS1307RTC.h> //https://www.pjrc.com/teensy/td_libs_DS1307RTC.html
|
||||
#include <TimeLord.h> //https://github.com/probonopd/TimeLord
|
||||
#include <TimeLib.h> //https://github.com/PaulStoffregen/Time
|
||||
#include <TimeAlarms.h> //https://github.com/PaulStoffregen/TimeAlarms
|
||||
|
||||
|
||||
// Pin definitions for LED indicators
|
||||
// ==================================
|
||||
#define AzLED 7 // Mega2560 D7
|
||||
#define AltLED 6 // Mega2560 D6
|
||||
#define FROMEQG 5 // Mega2560 D5
|
||||
#define FROMHBX 4 // Mega2560 D4
|
||||
|
||||
// Jumpers to run monitor or test
|
||||
// ==============================
|
||||
#define TESTHBX 9 // Mega2560 D2
|
||||
#define MONITORHBX 11 // Mega2560 D3
|
||||
|
||||
/**************************************************************
|
||||
* Common variables
|
||||
**************************************************************/
|
||||
|
||||
unsigned long DelayTimer = 0; // Delay timer
|
||||
unsigned long StatusTimer = 0; // H2X delay timer
|
||||
unsigned long StatusTime = 0; // H2X interval time
|
||||
|
||||
long P1;
|
||||
long P2;
|
||||
float F1;
|
||||
float F2;
|
||||
|
||||
time_t epoch; // UTC seconds
|
||||
time_t currentTime; // Local value
|
||||
|
||||
/**************************************************************
|
||||
* EQG protocol communications buffers and pointers
|
||||
* EQG protocol variables
|
||||
**************************************************************/
|
||||
|
||||
#define EQGLEN 256 // Communications buffers
|
||||
#define EQGMASK EQGLEN-1 // Index wraps to 0
|
||||
|
||||
unsigned long EQGP1;
|
||||
unsigned long EQGP2;
|
||||
float EQGF1;
|
||||
|
||||
unsigned char EQGRxBuffer[EQGLEN]; // Hold data from EQG
|
||||
unsigned char EQGTxBuffer[EQGLEN]; // Hold responses to EQG
|
||||
unsigned char EQGRxiPtr = 0; // Pointer for input from EQG
|
||||
unsigned char EQGRxoPtr = 0; // Pointer for output from EQG Rx buffer
|
||||
unsigned char EQGTxiPtr = 0; // Pointer for input to EQG buffer
|
||||
unsigned char EQGTxoPtr = 0; // Pointer for output to EQG
|
||||
|
||||
unsigned char EQGDbgBuffer[EQGLEN]; // Debug
|
||||
unsigned char EQGDbgPtr = 0; // Pointer for debug
|
||||
|
||||
unsigned char EQGCmnd = 0; // EQG Command
|
||||
unsigned char EQGErrorValue; // EQG Returned error value
|
||||
unsigned char EQGDone = 0; // EQG Complete message
|
||||
unsigned char EQGRADEC = 0; // EQG Motor Select (ASCII)
|
||||
unsigned char EQGMOTOR = 0; // EQG Motor Select (binary)
|
||||
unsigned char EQGRAAutoguide = 0; // EQG Autoguide rate
|
||||
unsigned char EQGDECAutoguide = 0; // EQG Autoguide rate
|
||||
unsigned char EQGRxState = 1; // EQG State
|
||||
unsigned char EQGRxChar; // EQG Rx Character
|
||||
unsigned char EQGRxCount; // EQG # parameters
|
||||
unsigned char EQGDbgCount; // EQG # parameters
|
||||
|
||||
/**************************************************************
|
||||
* HBX communications buffers and pointers
|
||||
* HBX variables
|
||||
**************************************************************/
|
||||
|
||||
unsigned long H2XStart = 0; // Used to count uS ticks
|
||||
unsigned long H2XTimer = 0; // Used to count uS ticks
|
||||
unsigned char MotorStatus; // Current State of motor
|
||||
|
||||
typedef struct {
|
||||
unsigned char MotorType; // Current type of motor
|
||||
unsigned char MotorFlag; // Flag to print motor positions
|
||||
|
||||
unsigned long ETXMotorState; // ETX Motor State Nachine
|
||||
unsigned long HBXMotorStatus; // Current HBX Motor State
|
||||
unsigned long HBXMotorControl; // Current HBX Motor Control bits
|
||||
|
||||
unsigned char HBXBitCount; // #bits left to process
|
||||
unsigned char HBXCmnd; // Current command
|
||||
unsigned char HBXData; // Data byte from HBX Bus
|
||||
unsigned char HBXP1; // HBX status/data - MSB
|
||||
unsigned char HBXP2; // HBX status/data
|
||||
unsigned char HBXP3; // HBX status/data - LSB
|
||||
unsigned char HBXP4; // HBX status/data - encoder error
|
||||
unsigned char HBXCount; // HBX valid data count
|
||||
unsigned char HBXLEDI; // LED current value from Motor
|
||||
unsigned long HBXDirSpeed; // Speed, Direction for Motor to move
|
||||
char HBXGuide; // Guide speed
|
||||
long HBXSpeed; // Move speed
|
||||
long HBXTargetSpeed; // Target Move speed
|
||||
char HBXSpeedState; // Slowdown/speedup state
|
||||
long HBXPosn; // Current position
|
||||
long HBXTarget; // Current target delta
|
||||
long HBXDelta; // Change in position for motor speed calcs
|
||||
long HBXSlowDown; // Point to change to lower speed
|
||||
long HBXOffset; // Current adjustment
|
||||
long HBX_bVALUE; // For rate calculations
|
||||
unsigned long SIDEREALRATE; // Constants
|
||||
unsigned long SOLARRATE;
|
||||
unsigned long LUNARRATE;
|
||||
unsigned long DEGREERATE1;
|
||||
unsigned long DEGREERATE2;
|
||||
unsigned long SIDEREALSPEED;
|
||||
unsigned long SOLARSPEED;
|
||||
unsigned long LUNARSPEED;
|
||||
unsigned long DEGREESPEED1;
|
||||
unsigned long DEGREESPEED2;
|
||||
unsigned long STEPSPER360;
|
||||
unsigned long WORM;
|
||||
unsigned long PEC;
|
||||
unsigned char PrintStatus0; // Force print of no status change
|
||||
unsigned long TimeDelta; // Used in HBX Monitor
|
||||
} axis_type;
|
||||
|
||||
axis_type axis[4]; // Az, Alt
|
||||
|
||||
|
||||
unsigned long PreviousTime; // Used in HBX Monitor, Testing
|
||||
|
||||
// Testing only
|
||||
unsigned char TestCount;
|
||||
unsigned long TestLoopTime;
|
||||
|
||||
// Monitor only
|
||||
unsigned char DetectedClock;
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,272 @@
|
|||
/*
|
||||
* Copyright 2017, 2018 John Archbold
|
||||
*/
|
||||
|
||||
/********************************************************
|
||||
Initialize HBX, translate EQG to HBX
|
||||
====================================
|
||||
*********************************************************/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <EEPROM.h>
|
||||
#include "EQGProtocol.h"
|
||||
#include "ETXProtocol.h"
|
||||
#include "HBXComms.h"
|
||||
#include "EQG2HBX.h" // All the declared variables
|
||||
|
||||
void setup()
|
||||
{
|
||||
int i, j, k;
|
||||
bool b;
|
||||
|
||||
DelayTimer = micros(); // Initialize timers, counts
|
||||
StatusTimer = DelayTimer;
|
||||
StatusTime = DelayTimer;
|
||||
TestCount = 0;
|
||||
EQGErrorValue = 0;
|
||||
|
||||
pinMode(FROMEQG, OUTPUT); // Initialize Indicator pins
|
||||
pinMode(FROMHBX, OUTPUT);
|
||||
pinMode(AzLED, OUTPUT);
|
||||
pinMode(AltLED, OUTPUT);
|
||||
digitalWrite(FROMEQG, LOW); // Initialize Indicator LEDS
|
||||
digitalWrite(FROMHBX, LOW);
|
||||
digitalWrite(AzLED, LOW);
|
||||
digitalWrite(AltLED, LOW);
|
||||
|
||||
pinMode(MONITORHBX, INPUT_PULLUP); // Initialize Operation jumpers
|
||||
pinMode(TESTHBX, INPUT_PULLUP);
|
||||
digitalWrite(MONITORHBX, HIGH);
|
||||
digitalWrite(TESTHBX, HIGH);
|
||||
|
||||
axis[AzMotor].PrintStatus0 = 0; // Disable printing status polls with no change
|
||||
axis[AltMotor].PrintStatus0 = 0; // Disable printing status polls with no change
|
||||
|
||||
// Initialize EQG communications
|
||||
Serial.begin(115200); // Debug
|
||||
Serial.println("ETX V5.01");
|
||||
Serial1.begin(9600); // EQMOD
|
||||
Serial1.println("ETX-EQMOD");
|
||||
Serial2.begin(9600);
|
||||
Serial3.begin(9600);
|
||||
|
||||
Serial.print("EEPROM length: ");
|
||||
Serial.println(EEPROM.length());
|
||||
Serial.print("CRC32 of EEPROM data: 0x");
|
||||
Serial.print(eeprom_crc(), HEX);
|
||||
if (!check_eeprom_crc()) {
|
||||
Serial.println(" - crc failed");
|
||||
set_eeprom_crc();
|
||||
check_eeprom_crc();
|
||||
}
|
||||
else Serial.println(" - crc OK");
|
||||
|
||||
|
||||
// **************************
|
||||
// Check for HBX Monitor Mode
|
||||
// ==========================
|
||||
if (digitalRead(MONITORHBX) == 0) { // Check if monitor jumper installed
|
||||
HBXMonitorLoop(); // Execute monitor code until jumper removed
|
||||
}
|
||||
|
||||
// **************************
|
||||
// Check for HBX Testing Mode
|
||||
// ==========================
|
||||
if (digitalRead(TESTHBX) == 0) { // Check if test jumper installed
|
||||
HBXTestLoop(); // Execute test code until jumper removed
|
||||
}
|
||||
|
||||
while (digitalRead(TESTHBX) == 0) {
|
||||
Serial.println("Test HBX commands to ETX");
|
||||
HBXTest();
|
||||
TimerDelaymS(5000);
|
||||
}
|
||||
|
||||
Serial.println("HBX Initialization");
|
||||
AzInitialise();
|
||||
AltInitialise();
|
||||
|
||||
// Initialize HBX communications as outputs
|
||||
// It will use H2X communications
|
||||
HCL1Talk(); // Set for Talking on RAClk
|
||||
HCL2Talk(); // Set for Talking on DECClk
|
||||
HDAListen();
|
||||
TimerDelaymS(STARTTIME);
|
||||
|
||||
// Reset the motors (RA and DEC)
|
||||
// and wait until both respond to a command
|
||||
Serial.println("Wait for both motors to start up");
|
||||
WaitForMotors();
|
||||
|
||||
// Get Motor Type from Az MC ( assume both same type of motor)
|
||||
|
||||
Serial.print("Get Motor Type: ");
|
||||
do {
|
||||
axis[AzMotor].MotorType = 0x00;
|
||||
if (HBXSendCommand(GetMotorType, AzMotor))
|
||||
axis[AzMotor].MotorType = HBXGetByte(AzMotor);
|
||||
} while (!axis[AzMotor].MotorType);
|
||||
axis[AltMotor].MotorType = axis[AzMotor].MotorType;
|
||||
Serial.println(axis[AltMotor].MotorType);
|
||||
|
||||
// Get the real LEDI values from EEPROM
|
||||
axis[AzMotor].HBXLEDI = EEPROM.read(EEPROMAzLEDI);
|
||||
axis[AltMotor].HBXLEDI = EEPROM.read(EEPROMAltLEDI);
|
||||
// Set LED values from EEPROM
|
||||
Serial.print("Set Encoder LED currents - Az: ");
|
||||
if (HBXSendCommand(SetLEDI, AzMotor))
|
||||
HBXSendByte(axis[AzMotor].HBXLEDI, AzMotor);
|
||||
if (HBXSendCommand(SetLEDI, AltMotor))
|
||||
HBXSendByte(axis[AltMotor].HBXLEDI, AltMotor);
|
||||
Serial.print(axis[AzMotor].HBXLEDI);
|
||||
Serial.print(", Alt: ");
|
||||
Serial.println(axis[AltMotor].HBXLEDI);
|
||||
|
||||
// Set the Offset Clear Command
|
||||
// Send HBXP1, HBXP2 - which were initialised to 0
|
||||
Serial.println("Reset any ETX offset bytes");
|
||||
if (HBXSendCommand(SetOffset, AzMotor))
|
||||
HBXSend2Bytes(AzMotor);
|
||||
TimerDelaymS(CMNDTIME);
|
||||
if (HBXSendCommand(SetOffset, AltMotor))
|
||||
HBXSend2Bytes(AltMotor);
|
||||
TimerDelaymS(CMNDTIME);
|
||||
|
||||
// Stop the motors (RA and DEC)
|
||||
Serial.println("Stop motors");
|
||||
do {
|
||||
P1 = 0;
|
||||
if (HBXSendCommand(Stop, AzMotor)) P1 += 1;
|
||||
if (HBXSendCommand(Stop, AltMotor)) P1 += 1;
|
||||
} while (P1 < 2);
|
||||
|
||||
// Read status
|
||||
Serial.println("Read Status");
|
||||
HBXGet2Status(); // Check and read both motor states
|
||||
|
||||
currentTime = now();
|
||||
Serial.println("Setup Complete. Listening for commands ..");
|
||||
// Print debug info every 5 s
|
||||
// --------------------------
|
||||
Alarm.timerRepeat(10, debugEQG); // Every 10 seconds
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
|
||||
/**************************************************************************************************
|
||||
* Check ETXState
|
||||
**************************************************************************************************/
|
||||
|
||||
if ((micros() - DelayTimer) > 6550) {
|
||||
DelayTimer = micros();
|
||||
HBXGet2Status(); // Polls motor position every 6.55mS
|
||||
}
|
||||
|
||||
ETXState(AzMotor); // Check the Az motor state
|
||||
ETXState(AltMotor); // Check the Alt motor state
|
||||
|
||||
/**************************************************************************************************
|
||||
* Process EQG comms - Rx Comms are interrupt driven
|
||||
**************************************************************************************************/
|
||||
|
||||
EQGRx(); // Check for comms from EQG
|
||||
EQGState(); // Check command state
|
||||
if (EQGDone) { // EQG receive complete, see what it is
|
||||
if (EQGErrorValue == 0) {
|
||||
EQGAction(); // and do it
|
||||
}
|
||||
else {
|
||||
EQGError(EQGErrorValue); // EQGAction() may set an error
|
||||
}
|
||||
}
|
||||
while (EQGTxoPtr != EQGTxiPtr) { // EQG send any response
|
||||
Serial1.write(EQGTxBuffer[EQGTxoPtr++]); // Output to EQG
|
||||
EQGTxoPtr &= EQGMASK;
|
||||
}
|
||||
Alarm.delay(0);
|
||||
} // End loop()
|
||||
|
||||
|
||||
/**************************************************************************************************
|
||||
* Read / Update ETX - Timer Driven
|
||||
**************************************************************************************************/
|
||||
void UpdateETX(void) {
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**********************************************
|
||||
Multiple 1mS delay
|
||||
***********************************************/
|
||||
|
||||
void TimerDelaymS(unsigned long d) {
|
||||
delay(d);
|
||||
}
|
||||
|
||||
/**********************************************
|
||||
Multiple 1uS delay
|
||||
***********************************************/
|
||||
|
||||
void TimerDelayuS(unsigned int d) {
|
||||
delayMicroseconds(d);
|
||||
}
|
||||
|
||||
/**********************************************
|
||||
EEPROM support
|
||||
***********************************************/
|
||||
|
||||
unsigned long eeprom_crc(void) {
|
||||
|
||||
const unsigned long crc_table[16] = {
|
||||
0x00000000, 0x1db71064, 0x3b6e20c8, 0x26d930ac,
|
||||
0x76dc4190, 0x6b6b51f4, 0x4db26158, 0x5005713c,
|
||||
0xedb88320, 0xf00f9344, 0xd6d6a3e8, 0xcb61b38c,
|
||||
0x9b64c2b0, 0x86d3d2d4, 0xa00ae278, 0xbdbdf21c
|
||||
};
|
||||
|
||||
unsigned long crc = ~0L;
|
||||
|
||||
for (int index = 0 ; index < (EEPROM.length()-4) ; ++index) {
|
||||
crc = crc_table[(crc ^ EEPROM[index]) & 0x0f] ^ (crc >> 4);
|
||||
crc = crc_table[(crc ^ (EEPROM[index] >> 4)) & 0x0f] ^ (crc >> 4);
|
||||
crc = ~crc;
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
unsigned long get_eeprom_crc(void) {
|
||||
unsigned long crc;
|
||||
int i;
|
||||
i = EEPROM.length()-4; // Location of stored crc (last four bytes)
|
||||
crc = 0;
|
||||
for (int j = 0; j < 4; j++) {
|
||||
crc = crc << 8;
|
||||
crc = crc | EEPROM.read(i);
|
||||
i += 1;
|
||||
}
|
||||
return (crc);
|
||||
}
|
||||
|
||||
bool set_eeprom_crc(void) {
|
||||
unsigned long crc;
|
||||
unsigned char data;
|
||||
int i;
|
||||
i = EEPROM.length()-1; // Location of stored crc (last four bytes)
|
||||
crc = eeprom_crc();
|
||||
for (int j = 0; j < 4; j++) {
|
||||
data = crc & 0xFF;
|
||||
EEPROM.write(i, data);
|
||||
if (EEPROM.read(i) != data) return (false);
|
||||
i -= 1;
|
||||
crc = crc >> 8;
|
||||
}
|
||||
return (true);
|
||||
}
|
||||
|
||||
bool check_eeprom_crc(void) {
|
||||
if (eeprom_crc() == get_eeprom_crc()) return true;
|
||||
else return false;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,83 @@
|
|||
/*
|
||||
* Copyright 2017, 2018 John Archbold
|
||||
*/
|
||||
|
||||
/********************************************************
|
||||
EQG Protocol function definitions
|
||||
=================================
|
||||
*********************************************************/
|
||||
#ifndef EQGProtocol
|
||||
#define EQGProtocol
|
||||
|
||||
#define CR 0x0d
|
||||
#define LF 0x0a
|
||||
|
||||
#define SIDEREALSECS 86164 // Some astronomical constants
|
||||
#define SOLARSECS 86400
|
||||
#define LUNARSECS 89309
|
||||
#define ARCSECS 360*60*60
|
||||
|
||||
#define EQG_CMNDSTART 0x01
|
||||
#define EQG_WAITFORCR 0x77
|
||||
#define EQG_INTERPRET 0x78
|
||||
|
||||
#define EQGVERSION 0x000501 // Simulate EQ6
|
||||
|
||||
#define EQG_CENTRE ETX_CENTRE
|
||||
#define EQG_RAbVALUE AzSIDEREALRATE*(AzSTEPSPER360/SIDEREALSECS)
|
||||
#define EQG_DECbVALUE AltSIDEREALRATE*(AzSTEPSPER360/SIDEREALSECS)
|
||||
#define EQG_gVALUE 0x00000010
|
||||
|
||||
#define EQGMAXIMUMSPEED 12 // 0x0C
|
||||
|
||||
// EQG 'G' Command - SET move parameters
|
||||
#define DIRECTION 0x0001 // N/E(0) S/W(1)
|
||||
//#define HEMISPHERE 0x0002 // North(0) South(1)
|
||||
//#define SLEWSTEP 0x0010 // Slew(0) Step(1)
|
||||
//#define HIGHLOW 0x0020 // High(0) Low(1)
|
||||
|
||||
// 3 HIGH SPEED SLEW
|
||||
// 2 LOW SPEED GOTO
|
||||
// 1 LOW SPEED SLEW
|
||||
// 0 HIGH SPEED GOTO
|
||||
|
||||
// EQG 'f' Command - GET Motor status bit definitions
|
||||
// MotorState bit definitions
|
||||
// nibble 1
|
||||
#define MOVESTEP 0x0001 // Slew(0) Step(1)
|
||||
#define MOVEDIRN 0x0002 // Decreasing(0) Increasing(1)
|
||||
#define MOVELOW 0x0004 // High(0) Low(1)
|
||||
// nibble2
|
||||
#define MOVEAXIS 0x0010 // Stopped(0) Moving(1)
|
||||
#define MOVEFACE 0x0020 // Front(0) Rear(1)
|
||||
// nibble3
|
||||
#define COILACTIVE 0x0100 // Inactive(0) Active(1)
|
||||
|
||||
#define FORWARD MOVEDIRN
|
||||
#define REVERSE 0
|
||||
|
||||
void EQGState(void);
|
||||
void EQGError(unsigned char);
|
||||
void EQGAction(void);
|
||||
|
||||
void TimerDelaymS(unsigned long);
|
||||
|
||||
void EQGRx(void);
|
||||
void EQGTx(unsigned char);
|
||||
void EQGTxHex(unsigned char);
|
||||
void EQGTxHex2(unsigned char);
|
||||
void EQGTxHex3(unsigned int);
|
||||
void EQGTxHex6(unsigned long);
|
||||
|
||||
// debug
|
||||
void putbyte(unsigned char);
|
||||
void putbyte(unsigned char);
|
||||
void puthexn(unsigned char);
|
||||
void puthexb(unsigned char);
|
||||
void puthexw(unsigned int);
|
||||
void puthexl(unsigned long);
|
||||
void putdecn(unsigned char);
|
||||
void putdecb(unsigned char);
|
||||
void putdecw(unsigned int);
|
||||
void putdecl(unsigned long);
|
||||
#endif
|
||||
|
|
@ -0,0 +1,555 @@
|
|||
/*
|
||||
* Copyright 2017, 2018 John Archbold
|
||||
*/
|
||||
|
||||
/********************************************************
|
||||
EQG Protocol related functions
|
||||
==============================
|
||||
*********************************************************/
|
||||
|
||||
// Process received EQG characters
|
||||
// ===============================
|
||||
void EQGState(void) {
|
||||
while ((EQGRxiPtr != EQGRxoPtr) && (EQGDone == 0)) {
|
||||
digitalWrite(FROMEQG, HIGH); // Set Indicator LED
|
||||
EQGRxChar = EQGRxBuffer[EQGRxoPtr++]; // Get a character
|
||||
if ((EQGRxState < EQG_WAITFORCR) && (EQGRxChar < ' ')) {
|
||||
EQGRxState = EQG_INTERPRET; // Terminate on non-alpha
|
||||
}
|
||||
|
||||
// Convert hex parameters to binary nibble
|
||||
// ---------------------------------------
|
||||
if ((EQGRxState > 0x03) && (EQGRxState < EQG_WAITFORCR)) {
|
||||
EQGRxChar = toupper(EQGRxChar);
|
||||
if ((EQGRxChar <= 'F') && (EQGRxChar >= '0')) {
|
||||
EQGRxChar -= '0';
|
||||
if (EQGRxChar > 9) EQGRxChar -= 0x07;
|
||||
}
|
||||
else EQGRxState = EQG_INTERPRET; // terminate on non-hex
|
||||
}
|
||||
|
||||
// Rx State machine
|
||||
// ----------------
|
||||
|
||||
switch (EQGRxState) {
|
||||
|
||||
case EQG_WAITFORCR: // Waiting for CR
|
||||
if (EQGRxChar == CR) {
|
||||
EQGRxState = EQG_CMNDSTART;
|
||||
EQGDone++;
|
||||
}
|
||||
break;
|
||||
|
||||
case EQG_CMNDSTART: // Waiting for ':'
|
||||
if (EQGRxChar == ':') {
|
||||
EQGRxCount = 1; // Count for # parameters
|
||||
EQGRxState++;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x02: // Waiting for command
|
||||
EQGCmnd = EQGRxChar;
|
||||
EQGRxCount++;
|
||||
EQGRxState++;
|
||||
break;
|
||||
|
||||
case 0x03: // Which motor?
|
||||
EQGRADEC = EQGRxChar;
|
||||
EQGMOTOR = EQGRADEC - '0';
|
||||
if ((EQGRADEC > '0') && (EQGRADEC < '3')) {
|
||||
EQGRxCount++;
|
||||
switch (EQGCmnd) { // Need data?
|
||||
case 'A': // Unknown
|
||||
case 'B': // Unknown
|
||||
case 'E': // Set Current Position
|
||||
case 'G': // Set Move direction and speed multiplier
|
||||
case 'H': // Set Target Position
|
||||
case 'I': // Set GOTO Speed
|
||||
case 'M': // Set Mount Motor Change Speed
|
||||
case 'P': // Set the AutoGuide Speed
|
||||
case 'R': // Unknown
|
||||
case 'S': // Unknown
|
||||
case 'T': // Unknown
|
||||
case 'U': // Unknown
|
||||
EQGRxState++; // Yes, so next state
|
||||
break;
|
||||
|
||||
default:
|
||||
EQGRxState = EQG_INTERPRET; // No, so command complete
|
||||
break;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (EQGRxChar != 0x0d) EQGRxState = EQG_WAITFORCR;
|
||||
else EQGDone++;
|
||||
EQGErrorValue = '3'; // Failure - Bad Parameters
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x04: // First nibble
|
||||
EQGP1 = EQGRxChar; // EQGRxChar already converted to binary nibble
|
||||
EQGRxCount++; // Increase character count
|
||||
switch (EQGCmnd) { // Need to get more data?
|
||||
case 'B': // B only takes one nibble
|
||||
case 'P': // P only takes one nibble
|
||||
EQGRxState = EQG_INTERPRET;
|
||||
break;
|
||||
default:
|
||||
EQGRxState++;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x05: // Second nibble - first byte (B1 = N1N2)
|
||||
EQGP2 = EQGRxChar; // EQGRxChar already converted to binary nibble
|
||||
EQGP1 = ((EQGP1 << 4) | EQGP2); // First byte
|
||||
EQGRxCount++;
|
||||
switch (EQGCmnd) { // Need to get more data?
|
||||
case 'A': // A unknown
|
||||
case 'G': // G Set direction, range
|
||||
case 'R': // R unknown
|
||||
EQGRxState = EQG_INTERPRET;
|
||||
break;
|
||||
default:
|
||||
EQGRxState++;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 0x06: // Third nibble - N3N1N2
|
||||
EQGP2 = EQGRxChar; // EQGRxChar already converted to binary nibble
|
||||
EQGP1 |= (EQGP2 << 8);
|
||||
EQGRxCount++;
|
||||
EQGRxState++;
|
||||
break;
|
||||
|
||||
case 0x07: // Fourth nibble - N3N4N1N2
|
||||
EQGP1 &= 0xFF; // Clear all bar low byte
|
||||
EQGP1 |= (EQGP2 << 12); // Get N3 into correct position
|
||||
EQGP2 = EQGRxChar; // EQGRxChar already converted to binary nibble
|
||||
EQGP1 |= (EQGP2 << 8); // Get N4 into correct position
|
||||
EQGRxCount++;
|
||||
EQGRxState++; // Get next data
|
||||
break;
|
||||
|
||||
case 0x08: // Fifth nibble - N5xxN3N4N1N2
|
||||
EQGP2 = EQGRxChar;
|
||||
EQGRxCount++;
|
||||
EQGP1 |= (EQGP2 << 20);
|
||||
EQGRxState++; // Get next data
|
||||
break;
|
||||
|
||||
case 0x09: // Sixth nibble - N5N6N3N4N1N2
|
||||
EQGP2 = EQGRxChar;
|
||||
EQGRxCount++;
|
||||
EQGP1 |= (EQGP2 << 16);
|
||||
EQGRxState = EQG_INTERPRET; // All done
|
||||
break;
|
||||
|
||||
case EQG_INTERPRET:
|
||||
if ((EQGRxChar == 0x0d) && (EQGRxCount >= 3)) {
|
||||
EQGRxState = EQG_CMNDSTART; // Reset state machine
|
||||
switch (EQGCmnd) {
|
||||
// Commands that need no data
|
||||
case 'a': // Read steps per rotation
|
||||
case 'b': // Read tracking scale
|
||||
case 'c': // Read Motor Speed Change
|
||||
case 'd': //
|
||||
case 'e': // Read firmware version
|
||||
case 'f': // Read motor status
|
||||
case 'g': // Read speed divisor
|
||||
case 'j': // Read position
|
||||
case 'm': // Read motor slowdown point
|
||||
case 's': // Read steps per arcsec
|
||||
case 'D': // Read track rate
|
||||
case 'F': // Initialize motors
|
||||
case 'J': // GoTo position, track
|
||||
case 'K': // Stop movement
|
||||
EQGDone++;
|
||||
break;
|
||||
// Commands that need data
|
||||
case 'E': // Set current position
|
||||
case 'H': // Set target position
|
||||
case 'I': // Set GoTo speed
|
||||
case 'M': // Set motor slowdown position
|
||||
if (EQGRxCount == (3 + 6)) {
|
||||
EQGDone++;
|
||||
}
|
||||
else {
|
||||
if (EQGRxChar != 0x0d) EQGRxState = EQG_WAITFORCR;
|
||||
else EQGDone++;
|
||||
EQGErrorValue = '3'; // Failure - Bad Parameters
|
||||
}
|
||||
break;
|
||||
|
||||
case 'G': // Set direction and range
|
||||
if (EQGRxCount == (3 + 2)) {
|
||||
EQGDone++;
|
||||
}
|
||||
else {
|
||||
if (EQGRxChar != 0x0d) EQGRxState = EQG_WAITFORCR;
|
||||
else EQGDone++;
|
||||
EQGErrorValue = '3'; // Failure - Bad Parameters
|
||||
}
|
||||
break;
|
||||
|
||||
case 'P': // Set autoguide speed
|
||||
if (EQGRxCount == (3 + 1)) {
|
||||
EQGDone++;
|
||||
}
|
||||
else {
|
||||
if (EQGRxChar != 0x0d) EQGRxState = EQG_WAITFORCR;
|
||||
else EQGDone++;
|
||||
EQGErrorValue = '3'; // Failure - Bad Parameters
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
if (EQGRxChar != 0x0d) EQGRxState = EQG_WAITFORCR;
|
||||
else EQGDone++;
|
||||
EQGErrorValue = '0'; // Failure - Bad Command
|
||||
break;
|
||||
} // End - switch (EQGCmnd)
|
||||
}
|
||||
else {
|
||||
if (EQGRxChar != 0x0d) EQGRxState = EQG_WAITFORCR;
|
||||
else EQGDone++;
|
||||
EQGErrorValue = '0'; // Failure - Bad Command
|
||||
}
|
||||
break; // End - if (((EQGRxChar == 0x0d) || (EQGRxChar == 0x0a)) && (EQGRxCount >= 3))
|
||||
|
||||
default:
|
||||
EQGRxState = EQG_CMNDSTART;
|
||||
} // End - switch (EQGRxState)
|
||||
} // END - while ((EQGRxiPtr != EQGRxoPtr) && (EQGDone == 0))
|
||||
digitalWrite(FROMEQG, LOW); // Clear Indicator LED
|
||||
}
|
||||
|
||||
void EQGError(unsigned char errorbyte) {
|
||||
EQGTx('!') ; // Failure - Bad Parameters
|
||||
EQGTx(errorbyte);
|
||||
EQGTx(CR);
|
||||
EQGDone = 0; // Process errors
|
||||
EQGRxState = EQG_CMNDSTART;
|
||||
EQGRxCount = 0; // Count for # parameters
|
||||
EQGErrorValue = 0;
|
||||
}
|
||||
|
||||
// Received a valid command so execute it, if supported
|
||||
// ====================================================
|
||||
void EQGAction(void) {
|
||||
|
||||
EQGDone = 0; // Reset flag
|
||||
EQGRxState = EQG_CMNDSTART;
|
||||
digitalWrite(FROMEQG, HIGH); // and status LED
|
||||
EQGTx('='); // Answer (with parameters)
|
||||
|
||||
switch (EQGCmnd) {
|
||||
|
||||
case 'a': // Request total number of steps per revolution
|
||||
EQGTxHex6(axis[EQGMOTOR].STEPSPER360);
|
||||
break;
|
||||
|
||||
case 'b': // Request step interrupt frequency
|
||||
EQGTxHex6(axis[EQGMOTOR].HBX_bVALUE); // Let EQMOD calculate SIDEREAL, etc
|
||||
break;
|
||||
|
||||
case 'c':
|
||||
EQGTxHex6(0x00D800); // Return same as EQ6 to keep the speed commands consistent
|
||||
break;
|
||||
|
||||
case 'd': // Request Mount center point
|
||||
EQGTxHex6(EQG_CENTRE); // Return same as EQ6 to keep the speed commands consistent
|
||||
break;
|
||||
|
||||
case 'e': // Request version
|
||||
EQGTxHex6(EQGVERSION); // V5.01
|
||||
break;
|
||||
|
||||
case 'f': // Request motor status
|
||||
|
||||
// MotorState bit definitions
|
||||
// nibble 1
|
||||
// MOVESTEP 0x0001 // Slew(0) Step(1)
|
||||
// MOVEDIRN 0x0002 // Decreasing(0) Increasing(1)
|
||||
// MOVELOW 0x0004 // Low(0) High(1)
|
||||
// nibble2
|
||||
// MOVEAXIS 0x0010 // Stopped(0) Moving(1)
|
||||
// MOVEFACE 0x0020 // Front(0) Rear(1)
|
||||
// nibble3
|
||||
// COILACTIVE 0x0100 // Inactive(0) Active(1)
|
||||
EQGTxHex3(axis[EQGMOTOR].HBXMotorStatus);
|
||||
|
||||
break;
|
||||
|
||||
case 'g': // Request speed multiplier
|
||||
EQGTxHex2(EQG_gVALUE); // Return same as EQ6 to keep the speed commands consistent
|
||||
break;
|
||||
|
||||
case 'j': // Request axis position
|
||||
EQGTxHex6(axis[EQGMOTOR].HBXPosn);
|
||||
break;
|
||||
|
||||
case 'm': // GET Point at which to change from fast to slow
|
||||
EQGTxHex6(axis[EQGMOTOR].HBXSlowDown);
|
||||
break;
|
||||
|
||||
case 's': // PEC period
|
||||
EQGTxHex6(axis[EQGMOTOR].PEC);
|
||||
break;
|
||||
|
||||
case 'E': // Set current motor position
|
||||
axis[EQGMOTOR].HBXPosn = EQGP1;
|
||||
break;
|
||||
|
||||
case 'F': // Initialize and activate motors
|
||||
axis[EQGMOTOR].HBXMotorStatus |= COILACTIVE;
|
||||
break;
|
||||
|
||||
case 'G': // EQG 'G' Command - SET move parameters
|
||||
// DIRECTION 0x0001 // N/E(0) S/W(1)
|
||||
// HEMISPHERE?? 0x0002 // North(0) South(1)
|
||||
|
||||
axis[EQGMOTOR].HBXDirSpeed = (int)EQGP1; // Save the actual value
|
||||
if (EQGP1 & DIRECTION) // Set direction bit
|
||||
axis[EQGMOTOR].HBXMotorStatus &= ~MOVEDIRN;
|
||||
else
|
||||
axis[EQGMOTOR].HBXMotorStatus |= MOVEDIRN;
|
||||
|
||||
// SLEWSTEP 0x0010 // Slew(0) Step(1)
|
||||
// HIGHLOW 0x0020 // High(0) Low(1)
|
||||
switch ((EQGP1 >> 4) & 0x03) {
|
||||
case 00: // 0 HIGH SPEED SLEW
|
||||
axis[EQGMOTOR].HBXMotorStatus &= ~MOVESTEP;
|
||||
axis[EQGMOTOR].HBXMotorStatus &= ~MOVELOW;
|
||||
break;
|
||||
case 01: // 1 HIGH SPEED STEP
|
||||
axis[EQGMOTOR].HBXMotorStatus |= MOVESTEP;
|
||||
axis[EQGMOTOR].HBXMotorStatus &= ~MOVELOW;
|
||||
break;
|
||||
case 02: // 2 LOW SPEED GOTO
|
||||
axis[EQGMOTOR].HBXMotorStatus &= ~MOVESTEP;
|
||||
axis[EQGMOTOR].HBXMotorStatus |= MOVELOW;
|
||||
break;
|
||||
case 03: // 3 LOW SPEED STEP
|
||||
axis[EQGMOTOR].HBXMotorStatus |= MOVESTEP;
|
||||
axis[EQGMOTOR].HBXMotorStatus |= MOVELOW;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case 'H': // SetGotoTargetIncrement(AXISID Axis, long StepsCount)
|
||||
axis[EQGMOTOR].HBXDelta = EQGP1;
|
||||
if ((axis[EQGMOTOR].HBXMotorStatus & MOVEDIRN) == FORWARD)
|
||||
axis[EQGMOTOR].HBXTarget = axis[EQGMOTOR].HBXPosn + axis[EQGMOTOR].HBXDelta; // add the relative target
|
||||
else
|
||||
axis[EQGMOTOR].HBXTarget = axis[EQGMOTOR].HBXPosn - axis[EQGMOTOR].HBXDelta; // subtract the relative target
|
||||
|
||||
Serial.println("");
|
||||
Serial.print("HBX:<");
|
||||
Serial.print(EQGMOTOR);
|
||||
Serial.print("-");
|
||||
Serial.print(axis[EQGMOTOR].HBXMotorStatus, HEX);
|
||||
Serial.print("> ");
|
||||
Serial.print(", Pos: ");
|
||||
Serial.print(axis[EQGMOTOR].HBXPosn, HEX);
|
||||
Serial.print(", Del: ");
|
||||
Serial.print(axis[EQGMOTOR].HBXDelta, HEX);
|
||||
Serial.print("->Tgt: ");
|
||||
Serial.print(axis[EQGMOTOR].HBXTarget, HEX);
|
||||
|
||||
axis[EQGMOTOR].HBXMotorControl |= MoveHBX; // Signal pending move
|
||||
break;
|
||||
|
||||
case 'I': // Set motor speed
|
||||
|
||||
if ( EQGP1 == 0x177C ) { // axis[EQGMOTOR].SIDEREALRATE) {
|
||||
EQGP1 = axis[EQGMOTOR].SIDEREALRATE;
|
||||
}
|
||||
else if ( EQGP1 == 0x1785 ) { // axis[EQGMOTOR].SOLARRATE) {
|
||||
EQGP1 = axis[EQGMOTOR].SOLARRATE;
|
||||
}
|
||||
else if ( EQGP1 == 0x185B ) { // axis[EQGMOTOR].LUNARRATE) {
|
||||
EQGP1 = axis[EQGMOTOR].LUNARRATE;
|
||||
}
|
||||
else {
|
||||
if (axis[EQGMOTOR].HBXMotorStatus & MOVELOW) { // 1 -> HIGH
|
||||
// EQG is (400, 480, 600, 800)
|
||||
// value: 18, 14, 10, 0c
|
||||
// Map 800 to 2 degrees / sec
|
||||
|
||||
EQGF1 = (float)EQGMAXIMUMSPEED / (float)EQGP1; // Scale to maximum EQG speed
|
||||
EQGF1 = EQGF1 * (float)axis[EQGMOTOR].DEGREESPEED2; // Multiply by maximum ETX speed
|
||||
}
|
||||
else { // EQG is (.5, 1, 2, 8, .. 64) * Sidereal
|
||||
EQGF1 = EQGF1 * (float)axis[EQGMOTOR].SIDEREALSPEED; // Multiply by Sidereal ETX speed
|
||||
}
|
||||
EQGP1 = (unsigned long) EQGF1; // Convert to ETX format
|
||||
EQGP2 = (unsigned long) ((EQGF1 - float(EQGP1)) * 65536L);
|
||||
EQGP1 = ((EQGP1 << 16) & 0xFF0000); // Format to 8.16 rate
|
||||
EQGP1 |= (EQGP2 & 0xFFFF);
|
||||
}
|
||||
axis[EQGMOTOR].HBXSpeed = EQGP1; // Set the speed
|
||||
break;
|
||||
|
||||
case 'J': // Tell motor to Go
|
||||
axis[EQGMOTOR].HBXMotorStatus |= MOVEAXIS; // Signal moving
|
||||
break;
|
||||
|
||||
case 'K': // Tell motor to stop
|
||||
axis[EQGMOTOR].ETXMotorState = ETXStopMotor;
|
||||
break;
|
||||
|
||||
case 'M': // SetBreakPointIncrement(AXISID Axis, long StepsCount)
|
||||
|
||||
axis[EQGMOTOR].HBXSlowDown = EQGP1;
|
||||
if ((axis[EQGMOTOR].HBXMotorStatus & MOVEDIRN) == FORWARD)
|
||||
axis[EQGMOTOR].HBXSlowDown = axis[EQGMOTOR].HBXTarget + axis[EQGMOTOR].HBXSlowDown; // add the relative target
|
||||
else
|
||||
axis[EQGMOTOR].HBXSlowDown = axis[EQGMOTOR].HBXTarget - axis[EQGMOTOR].HBXSlowDown; // subtract the relative target
|
||||
|
||||
Serial.println("");
|
||||
Serial.print("HBX:<");
|
||||
Serial.print(EQGMOTOR);
|
||||
Serial.print("-");
|
||||
Serial.print(axis[EQGMOTOR].HBXMotorStatus, HEX);
|
||||
Serial.print("> ");
|
||||
Serial.print(", Pos: ");
|
||||
Serial.print(axis[EQGMOTOR].HBXPosn, HEX);
|
||||
Serial.print(", Del: ");
|
||||
Serial.print(EQGP1, HEX);
|
||||
Serial.print("-> SD: ");
|
||||
Serial.println(axis[EQGMOTOR].HBXSlowDown, HEX);
|
||||
|
||||
break;
|
||||
|
||||
case 'P':
|
||||
axis[EQGMOTOR].HBXGuide = (char)EQGP1;
|
||||
break;
|
||||
|
||||
default:
|
||||
EQGErrorValue = '0'; // Failure - Bad Command
|
||||
break;
|
||||
}
|
||||
EQGTx(CR);
|
||||
digitalWrite(FROMEQG, LOW); // Clear Indicator LED
|
||||
}
|
||||
|
||||
/**********************************************
|
||||
Handle EQG communications
|
||||
***********************************************/
|
||||
void EQGRx(void) {
|
||||
while (Serial1.available() > 0) {
|
||||
EQGRxBuffer[EQGRxiPtr++] = Serial1.read();
|
||||
EQGRxiPtr &= EQGMASK;
|
||||
}
|
||||
}
|
||||
|
||||
void EQGTx(unsigned char data) {
|
||||
EQGTxBuffer[EQGTxiPtr++] = data;
|
||||
EQGTxiPtr &= EQGMASK;
|
||||
}
|
||||
|
||||
void EQGTxHex(unsigned char data) {
|
||||
if ((data & 0x0f) < 0x0a) EQGTx((data & 0x0f) + 0x30);
|
||||
else EQGTx((data & 0x0f) + 0x37);
|
||||
}
|
||||
|
||||
void EQGTxHex2(unsigned char data) {
|
||||
EQGTxHex(data >> 4);
|
||||
EQGTxHex(data);
|
||||
}
|
||||
|
||||
void EQGTxHex3(unsigned int data) {
|
||||
EQGTxHex( (unsigned char) data);
|
||||
EQGTxHex( (unsigned char) (data >> 4));
|
||||
EQGTxHex( (unsigned char) (data >> 8));
|
||||
}
|
||||
|
||||
void EQGTxHex6(unsigned long data) {
|
||||
EQGTxHex2( (unsigned char) data);
|
||||
EQGTxHex2( (unsigned char) (data >> 8));
|
||||
EQGTxHex2( (unsigned char) (data >> 16));
|
||||
}
|
||||
|
||||
/**********************************************
|
||||
Debug routines
|
||||
***********************************************/
|
||||
void putbyte(unsigned char data) {
|
||||
Serial.write(data);
|
||||
}
|
||||
|
||||
void puthexb(unsigned char data) {
|
||||
if (((data>>4) & 0x0f) < 0x0a) putbyte(((data>>4) & 0x0f) + 0x30);
|
||||
else putbyte(((data>>4) & 0x0f) + 0x37);
|
||||
if ((data & 0x0f) < 0x0a) putbyte((data & 0x0f) + 0x30);
|
||||
else putbyte((data & 0x0f) + 0x37);
|
||||
}
|
||||
|
||||
void putdecb(unsigned char data) {
|
||||
Serial.print(data);
|
||||
}
|
||||
|
||||
void puthexw(unsigned int data) {
|
||||
puthexb((data>>8) & 0xFF);
|
||||
puthexb(data & 0xFF);
|
||||
}
|
||||
|
||||
void putdecw(unsigned int data) {
|
||||
Serial.print(data);
|
||||
}
|
||||
|
||||
void puthex6(unsigned long data) {
|
||||
puthexb((data>>16) & 0xFF);
|
||||
puthexw(data & 0xFFFF);
|
||||
}
|
||||
|
||||
void puthexl(unsigned long data) {
|
||||
puthexw((data>>16) & 0xFFFF);
|
||||
puthexw(data & 0xFFFF);
|
||||
}
|
||||
|
||||
void putdecl(unsigned long data) {
|
||||
Serial.print(data);
|
||||
}
|
||||
|
||||
void EQGSend(unsigned char data) {
|
||||
Serial.write(data);
|
||||
}
|
||||
|
||||
void EQGSendHex(unsigned char data) {
|
||||
if ((data & 0x0f) < 0x0a) EQGSend((data & 0x0f) + 0x30);
|
||||
else EQGSend((data & 0x0f) + 0x37);
|
||||
}
|
||||
|
||||
void EQGSendHex2(unsigned char data) {
|
||||
EQGSendHex(data >> 4);
|
||||
EQGSendHex(data);
|
||||
}
|
||||
|
||||
void EQGSendHex6(unsigned long data) {
|
||||
EQGSendHex2( (unsigned char) data);
|
||||
EQGSendHex2( (unsigned char) (data >> 8));
|
||||
EQGSendHex2( (unsigned char) (data >> 16));
|
||||
}
|
||||
|
||||
void debugEQG() {
|
||||
Serial.println("");
|
||||
Serial.print("Az:<");
|
||||
Serial.print(axis[AzMotor].HBXMotorStatus, HEX);
|
||||
Serial.print(">Pos: ");
|
||||
Serial.print(axis[AzMotor].HBXPosn, HEX);
|
||||
Serial.print(" SD: ");
|
||||
Serial.print(axis[AzMotor].HBXSlowDown, HEX);
|
||||
Serial.print(" Tgt: ");
|
||||
Serial.print(axis[AzMotor].HBXTarget, HEX);
|
||||
|
||||
Serial.print(", Alt:<");
|
||||
Serial.print(axis[AzMotor].HBXMotorStatus, HEX);
|
||||
Serial.print(">Pos: ");
|
||||
Serial.print(axis[AltMotor].HBXPosn, HEX);
|
||||
Serial.print(" SD: ");
|
||||
Serial.print(axis[AltMotor].HBXSlowDown, HEX);
|
||||
Serial.print(" Tgt: ");
|
||||
Serial.print(axis[AltMotor].HBXTarget, HEX);
|
||||
}
|
||||
|
|
@ -0,0 +1,116 @@
|
|||
/*
|
||||
* Copyright 2017, 2018 John Archbold
|
||||
*/
|
||||
|
||||
|
||||
/********************************************************
|
||||
EQG Protocol function definitions
|
||||
=================================
|
||||
*********************************************************/
|
||||
#ifndef ETXProtocol
|
||||
#define ETXProtocol
|
||||
|
||||
#define MotorAz 0x01 // Pin3 on HBX interface
|
||||
#define MotorAlt 0x02 // Pin5 on HBX interface
|
||||
|
||||
#define AzMotor MotorAz
|
||||
#define AltMotor MotorAlt
|
||||
|
||||
// ETX Bit Definitions
|
||||
// Variable - axis[Motor].HBXMotorControl
|
||||
// nibble 4
|
||||
#define StartHBX 0x8000 // Motor start bit
|
||||
#define StopHBX 0x4000 // Motor stop bit
|
||||
#define MoveHBX 0x2000 // Move pending
|
||||
#define SpeedHBX 0x1000 // Speed change pending
|
||||
#define GoToHBX 0x0800 // GoTo in Progress
|
||||
|
||||
// ETX Known Commands
|
||||
#define RotateSlow 0x00 // Output "8.16" speed
|
||||
#define RotateFast 0x01 // Output "16.8" ? speed
|
||||
#define SetOffset 0x02 // Output "16" correction offset
|
||||
#define SetLEDI 0x03 // Output "8" LED current
|
||||
#define CalibrateLED 0x04 // None
|
||||
#define Stop 0x05 // None
|
||||
#define SlewReverse 0x06 // None
|
||||
#define SlewForward 0x07 // None
|
||||
#define GetStatus 0x08 // Input "16.8.1" ticks.pwm.error
|
||||
#define GetLEDI 0x09 // Input "8" LED current
|
||||
#define GetMotorType 0x0B // Input "8" Motor type
|
||||
#define ResetH2X 0xE4 // None
|
||||
|
||||
// ETX State Machine
|
||||
#define ETXCheckStartup 1
|
||||
#define ETXSlewMotor 2
|
||||
#define ETXStepMotor 3
|
||||
#define ETXCheckSlowDown 4
|
||||
#define ETXCheckSpeed 5
|
||||
#define ETXCheckPosition 6
|
||||
#define ETXCheckStop 7
|
||||
#define ETXStopMotor 8
|
||||
#define ETXMotorEnd 9
|
||||
|
||||
#define EEPROMAzLEDI 0x01 // EEPROM Storage
|
||||
#define EEPROMAltLEDI 0x02
|
||||
#define EEPROMMotor 0x03 // MOTOR TYPE
|
||||
|
||||
#define ETX60PERIOD 152.587891 // (1/6.55mS)
|
||||
|
||||
#define ETX_CENTRE 0x00800000
|
||||
#define AzSTEPSPER360 1233750 // See etx60at-gears.ods
|
||||
#define AltSTEPSPER360 1315440
|
||||
#define AzWORM 94
|
||||
#define AltWORM 58
|
||||
#define AzPEC AzSTEPSPER360/AzWORM
|
||||
#define AltPEC AltSTEPSPER360/AltWORM
|
||||
|
||||
#define AzSIDEREALRATE 0x001805 // (AzSTEPSPER360/SIDEREALSECS)/ETX60PERIOD
|
||||
#define AzSOLARRATE 0x0017F5 // (AzSTEPSPER360/SOLARSECS)/ETX60PERIOD
|
||||
#define AzLUNARRATE 0x00172D // (AzSTEPSPER360/LUNARSECS)/ETX60PERIOD
|
||||
#define AzDEGREERATE1 0x1675B1 // (AltSTEPSPER360/360)/ETX60PERIOD
|
||||
#define AzDEGREERATE2 AzDEGREERATE1 << 1
|
||||
|
||||
#define AltSIDEREALRATE 0x00199C // (AltSTEPSPER360/SIDEREALSECS)/ETX60PERIOD
|
||||
#define AltSOLARRATE 0x00198B // (AzSTEPSPER360/SOLARSECS)/ETX60PERIOD
|
||||
#define AltLUNARRATE 0x0018B6 // (AzSTEPSPER360/LUNARSECS)/ETX60PERIOD
|
||||
#define AltDEGREERATE1 0x17F265 // (AzSTEPSPER360/360)/ETX60PERIOD
|
||||
#define AltDEGREERATE2 AltDEGREERATE1 << 1
|
||||
|
||||
#define AzSIDEREALSPEED 0.09383854 // (AzSTEPSPER360/SIDEREALSECS)/ETX60PERIOD
|
||||
#define AzSOLARSPEED 0.09358222 // (AzSTEPSPER360/SOLARSECS)/ETX60PERIOD
|
||||
#define AzLUNARSPEED 0.09053403 // (AzSTEPSPER360/LUNARSECS)/ETX60PERIOD
|
||||
#define AzDEGREESPEED1 22.4597 // (AzSTEPSPER360/360)/ETX60PERIOD
|
||||
#define AzDEGREESPEED2 AzDEGREESPEED1 * 2.0
|
||||
|
||||
#define AltSIDEREALSPEED 0.10005185 // (AltSTEPSPER360/SIDEREALSECS)/ETX60PERIOD
|
||||
#define AltSOLARSPEED 0.09977856 // (AzSTEPSPER360/SOLARSECS)/ETX60PERIOD
|
||||
#define AltLUNARSPEED 0.09652854 // (AzSTEPSPER360/LUNARSECS)/ETX60PERIOD
|
||||
#define AltDEGREESPEED1 23.9469 // (AltSTEPSPER360/360)/ETX60PERIOD
|
||||
#define AltDEGREESPEED2 AltDEGREESPEED1 * 2.0
|
||||
|
||||
#define ETXSlew1 1 // 1 x sidereal (0.25 arc-min/sec or 0.0042°/sec)
|
||||
#define ETXSlew2 2 // 2 x sidereal (0.50 arc-min/sec or 0.0084°/sec)
|
||||
#define ETXSlew3 8 // 8 x sidereal ( 2 arc-min/sec or 0.0334°/sec)
|
||||
#define ETXSlew4 16 // 16 x sidereal ( 4 arc-min/sec or 0.0669°/sec)
|
||||
#define ETXSlew5 64 // 64 x sidereal ( 16 arc-min/sec or 0.2674°/sec)
|
||||
#define ETXSlew6 120 // 30 arc-min/sec or 0.5°/sec
|
||||
#define ETXSlew7 240 // 60 arc-min/sec or 1.0°/sec
|
||||
#define ETXSlew8 360 // 90 arc-min/sec or 1.5°/sec
|
||||
#define ETXSlew9 1080 // 270 arc-min/sec or 4.5°/sec
|
||||
|
||||
#define ETXSLOWPOSN 0x00000800 // Point at which to start slowdown
|
||||
|
||||
#define H2X_INPUTPU INPUT_PULLUP // Set pin data input mode
|
||||
#define H2X_INPUT INPUT // Set pin data input mode
|
||||
#define H2X_OUTPUT OUTPUT // Set pin data output
|
||||
|
||||
bool HBXSetMotorState(unsigned char);
|
||||
bool HBXCheckTargetStatus(unsigned char);
|
||||
bool HBXUpdatePosn(void);
|
||||
|
||||
bool HBXStartMotor(unsigned char);
|
||||
bool HBXStopMotor(unsigned char);
|
||||
bool HBXGetStatus(unsigned char);
|
||||
void HBXPosnPoll(unsigned char);
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,344 @@
|
|||
/*
|
||||
* Copyright 2017, 2018 John Archbold
|
||||
*/
|
||||
|
||||
|
||||
/********************************************************
|
||||
ETX Protocol related functions
|
||||
==============================
|
||||
*********************************************************/
|
||||
|
||||
bool ETXState(unsigned char Motor) {
|
||||
|
||||
long c, d;
|
||||
long l1;
|
||||
int s1;
|
||||
|
||||
switch(axis[Motor].ETXMotorState) {
|
||||
|
||||
case ETXCheckStartup:
|
||||
if (axis[Motor].HBXMotorStatus & MOVEAXIS) { // Are we moving?
|
||||
if (axis[EQGMOTOR].HBXMotorStatus & MOVESTEP) { // Step or Slew
|
||||
axis[Motor].ETXMotorState = ETXStepMotor;
|
||||
}
|
||||
else {
|
||||
if (axis[Motor].HBXDelta > 0x800) // Slew large enough for ETX slew?
|
||||
axis[Motor].ETXMotorState = ETXSlewMotor; // Yes, go
|
||||
else {
|
||||
axis[EQGMOTOR].HBXMotorStatus |= MOVESTEP; // No, change to step
|
||||
if (axis[EQGMOTOR].HBXDelta < 0x100) { // Scale speed down for really small slews
|
||||
axis[Motor].HBXTargetSpeed = axis[Motor].HBXTargetSpeed >> 2;
|
||||
}
|
||||
axis[Motor].HBXSpeed = 0;
|
||||
axis[Motor].ETXMotorState = ETXStepMotor;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ETXSlewMotor:
|
||||
if (Motor == MotorAz) digitalWrite(AzLED, HIGH); // Turn on the LED
|
||||
else digitalWrite(AltLED, HIGH);
|
||||
if ((axis[Motor].HBXMotorStatus & MOVEDIRN) == FORWARD)
|
||||
axis[Motor].HBXCmnd = SlewForward;
|
||||
else
|
||||
axis[Motor].HBXCmnd = SlewReverse;
|
||||
if (!(HBXSendCommand(axis[Motor].HBXCmnd, Motor))) {
|
||||
axis[Motor].ETXMotorState = ETXCheckStartup; // Motor failed to start
|
||||
}
|
||||
else
|
||||
axis[Motor].ETXMotorState = ETXCheckSlowDown;
|
||||
break;
|
||||
|
||||
case ETXStepMotor:
|
||||
if (Motor == MotorAz) digitalWrite(AzLED, HIGH); // Turn on the LED
|
||||
else digitalWrite(AltLED, HIGH);
|
||||
if (axis[Motor].HBXMotorStatus & MOVELOW) // Stepping, High or Low speed
|
||||
axis[Motor].HBXCmnd = RotateSlow;
|
||||
else axis[Motor].HBXCmnd = RotateFast;
|
||||
if (axis[Motor].HBXSpeed == 0) { // Starting up
|
||||
axis[Motor].HBXSpeed = axis[Motor].HBXTargetSpeed >> 2;
|
||||
}
|
||||
|
||||
// Set the speed, and direction
|
||||
// ----------------------------
|
||||
P1 = axis[Motor].HBXSpeed;
|
||||
if ((axis[Motor].HBXMotorStatus & MOVEDIRN) == REVERSE) // If negative, change P
|
||||
P1 = TwosComplement(P1); // to 2's complement
|
||||
axis[Motor].HBXP1 = (P1 >> 16) & 0xFF; // Initialize command bytes
|
||||
axis[Motor].HBXP2 = (P1 >> 8) & 0xFF;
|
||||
axis[Motor].HBXP3 = P1 & 0xFF;
|
||||
|
||||
// Send the command
|
||||
// ----------------
|
||||
if (HBXSendCommand(axis[Motor].HBXCmnd, Motor)) // Command OK?
|
||||
HBXSend3Bytes(Motor); // Send the speed
|
||||
if (axis[Motor].HBXMotorControl & MoveHBX) {
|
||||
axis[Motor].ETXMotorState = ETXCheckSpeed; // For a position move
|
||||
}
|
||||
else {
|
||||
axis[Motor].ETXMotorState = ETXCheckStartup; // Just a speed command
|
||||
axis[Motor].HBXMotorStatus &= ~MOVEAXIS; // Speed set, all done
|
||||
}
|
||||
break;
|
||||
|
||||
case ETXCheckSlowDown:
|
||||
// Check if Slowdown reached
|
||||
// Ramp-up speed til we get to slowdown point
|
||||
if ((axis[Motor].HBXMotorStatus & MOVEDIRN) == FORWARD) { // Moving forwards
|
||||
d = axis[Motor].HBXSlowDown;
|
||||
c = axis[Motor].HBXPosn;
|
||||
}
|
||||
else { // Moving backwards
|
||||
d = axis[Motor].HBXPosn;
|
||||
c = axis[Motor].HBXSlowDown;
|
||||
}
|
||||
if ((c >= d) || ((d - c) < ETXSLOWPOSN)) { // Check distance to slowdown
|
||||
axis[Motor].HBXSpeed = axis[Motor].DEGREERATE2; // Default slowdown 1st speed
|
||||
axis[Motor].HBXTargetSpeed = 0; // Default slowdown target i.e. stopped
|
||||
axis[Motor].HBXSpeedState = 0; // Speed ramp state
|
||||
axis[Motor].HBXMotorStatus |= MOVESTEP; // Set the step flag
|
||||
axis[Motor].ETXMotorState = ETXStepMotor; // GoTo speed ramp
|
||||
}
|
||||
break;
|
||||
|
||||
case ETXCheckSpeed:
|
||||
// Speeding Up
|
||||
// ===========
|
||||
if ((axis[Motor].HBXTargetSpeed != 0) && (axis[Motor].HBXTargetSpeed > axis[Motor].HBXSpeed)) {
|
||||
if ((axis[Motor].HBXTargetSpeed - axis[Motor].HBXSpeed) >= axis[Motor].DEGREERATE1) {
|
||||
axis[Motor].HBXSpeed += ((axis[Motor].HBXTargetSpeed - axis[Motor].HBXSpeed) >> 2);
|
||||
axis[Motor].ETXMotorState = ETXStepMotor;
|
||||
}
|
||||
else {
|
||||
axis[Motor].HBXSpeed = axis[Motor].HBXTargetSpeed;
|
||||
axis[Motor].ETXMotorState = ETXCheckPosition;
|
||||
}
|
||||
}
|
||||
// Slowing Down
|
||||
// ============
|
||||
else {
|
||||
if ((axis[Motor].HBXSpeed - axis[Motor].HBXTargetSpeed) >= axis[Motor].DEGREERATE1) {
|
||||
axis[Motor].HBXSpeed -= ((axis[Motor].HBXSpeed - axis[Motor].HBXTargetSpeed) >> 2);
|
||||
axis[Motor].ETXMotorState = ETXStepMotor;
|
||||
}
|
||||
else
|
||||
axis[Motor].ETXMotorState = ETXCheckPosition;
|
||||
}
|
||||
break;
|
||||
|
||||
case ETXCheckPosition:
|
||||
// Check if Target acquired
|
||||
// ------------------------
|
||||
|
||||
// Calculate absolute distance to target
|
||||
// -------------------------------------
|
||||
if (axis[Motor].HBXMotorControl & MoveHBX) {
|
||||
if ((axis[Motor].HBXMotorStatus & MOVEDIRN) == REVERSE) {
|
||||
d = axis[Motor].HBXTarget;
|
||||
c = axis[Motor].HBXPosn;
|
||||
}
|
||||
else {
|
||||
c = axis[Motor].HBXTarget;
|
||||
d = axis[Motor].HBXPosn;
|
||||
}
|
||||
l1 = c - d;
|
||||
|
||||
if (l1 >= 0) {
|
||||
// Start to slow motor if getting near target
|
||||
// ------------------------------------------
|
||||
if ((l1 <= 0x400) && (axis[Motor].HBXSpeedState == 0)) {
|
||||
axis[Motor].HBXSpeed = axis[Motor].HBXSpeed >> 1; // 1/2
|
||||
axis[Motor].ETXMotorState = ETXStepMotor; // Change speed
|
||||
axis[Motor].HBXSpeedState += 1;
|
||||
}
|
||||
else if ((l1 <= 0x100) && (axis[Motor].HBXSpeedState == 1)) {
|
||||
axis[Motor].HBXSpeed = axis[Motor].HBXSpeed >> 1; // 1/4
|
||||
axis[Motor].ETXMotorState = ETXStepMotor; // Change speed
|
||||
axis[Motor].HBXSpeedState += 1;
|
||||
}
|
||||
else if ((l1 <= 0x40) && (axis[Motor].HBXSpeedState == 2)) {
|
||||
axis[Motor].HBXSpeed = axis[Motor].HBXSpeed >> 2; // 1/16
|
||||
axis[Motor].ETXMotorState = ETXStepMotor; // Change speed;
|
||||
axis[Motor].HBXSpeedState += 1;
|
||||
}
|
||||
else if ((l1 <= 0x20) && (axis[Motor].HBXSpeedState == 3)) {
|
||||
axis[Motor].HBXSpeed = axis[Motor].HBXSpeed >> 2; // 1/64
|
||||
axis[Motor].ETXMotorState = ETXStepMotor; // Change speed;
|
||||
axis[Motor].HBXSpeedState += 1;
|
||||
}
|
||||
else if (l1 <= 0x08) {
|
||||
axis[Motor].ETXMotorState = ETXStopMotor; // Stop motor, next state
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Motor has over-shot the target
|
||||
// ------------------------------
|
||||
if ((axis[Motor].HBXMotorStatus & MOVEDIRN) == FORWARD) // EQG -> change direction
|
||||
axis[Motor].HBXMotorStatus &= ~FORWARD;
|
||||
else
|
||||
axis[Motor].HBXMotorStatus |= FORWARD;
|
||||
// axis[Motor].HBXSpeed = TwosComplement(axis[Motor].HBXSpeed); // ETX invert speed
|
||||
axis[Motor].ETXMotorState = ETXStepMotor; // Change ETX speed
|
||||
}
|
||||
}
|
||||
else {
|
||||
axis[Motor].ETXMotorState = ETXStopMotor;
|
||||
}
|
||||
break;
|
||||
|
||||
case ETXStopMotor:
|
||||
while(!(HBXSendCommand(Stop, Motor))); // Stop the motor
|
||||
axis[Motor].ETXMotorState = ETXMotorEnd; // Final state
|
||||
break;
|
||||
|
||||
case ETXMotorEnd:
|
||||
if (Motor == MotorAz) digitalWrite(AzLED, LOW); // Turn off the LED
|
||||
else digitalWrite(AltLED, LOW);
|
||||
if (axis[Motor].HBXMotorControl & MoveHBX) { // if we were moving
|
||||
// Check we got there exactly
|
||||
// --------------------------
|
||||
if ((axis[Motor].HBXMotorStatus & MOVEDIRN) == REVERSE) {
|
||||
d = axis[Motor].HBXTarget;
|
||||
c = axis[Motor].HBXPosn;
|
||||
}
|
||||
else {
|
||||
c = axis[Motor].HBXTarget;
|
||||
d = axis[Motor].HBXPosn;
|
||||
}
|
||||
l1 = c - d;
|
||||
|
||||
// Set the offset
|
||||
// ----------------------------
|
||||
if (l1 != 0) {
|
||||
axis[Motor].HBXP1 = (l1 >> 8) & 0xFF; // Initialize offset bytes
|
||||
axis[Motor].HBXP2 = l1 & 0xFF;
|
||||
axis[Motor].HBXCmnd = SetOffset;
|
||||
if (HBXSendCommand(axis[Motor].HBXCmnd, Motor)) // Command OK?
|
||||
HBXSend2Bytes(Motor); // Send the offset
|
||||
}
|
||||
axis[Motor].HBXPosn = axis[Motor].HBXTarget;
|
||||
axis[Motor].HBXMotorControl &= ~MoveHBX; // Clear the flag
|
||||
}
|
||||
axis[Motor].HBXMotorStatus |= MOVESTEP; // Set stepping mode
|
||||
axis[Motor].HBXMotorStatus &= ~MOVELOW; // and speed
|
||||
axis[Motor].HBXMotorStatus &= ~MOVEAXIS; // Clear the motor moving flag
|
||||
axis[Motor].ETXMotorState = ETXCheckStartup;
|
||||
axis[Motor].HBXTargetSpeed = axis[Motor].DEGREERATE1 >> 3; // For any subsequent move
|
||||
axis[Motor].HBXSpeed = 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Motor functions
|
||||
|
||||
bool HBXGetStatus(unsigned char Motor) {
|
||||
axis[Motor].HBXP4 = 0xFF; // Preset error
|
||||
if (!HBXSendCommand(GetStatus, Motor)) {
|
||||
return(false);
|
||||
}
|
||||
HBXGet3Bytes(Motor);
|
||||
if (axis[Motor].HBXP4 == 0) { // If no error, update position
|
||||
P1 = (axis[Motor].HBXP1 << 8);
|
||||
P1 |= axis[Motor].HBXP2; // Convert to 16bits
|
||||
if (axis[Motor].HBXP1 & 0x80)
|
||||
P1 |= 0xffff0000; // Sign extend HBXP1 for 2s complement
|
||||
axis[Motor].HBXPosn += P1;
|
||||
axis[Motor].HBXPosn &= 0x00FFFFFF;
|
||||
return(true);
|
||||
}
|
||||
else return(false);
|
||||
}
|
||||
|
||||
bool HBXGet2Status(void) {
|
||||
int i;
|
||||
do {
|
||||
i = 0;
|
||||
if (HBXGetStatus(AzMotor))
|
||||
i += 1;
|
||||
if (HBXGetStatus(AltMotor))
|
||||
i += 1;
|
||||
} while (i < 2);
|
||||
return(true);
|
||||
}
|
||||
|
||||
void WaitForMotors(void) {
|
||||
// GetLED commands always return a vaild value - motors not online until this happens
|
||||
// Valid values are not 0 and not 0xFF for Az, Alt. (exception here is if LEDRA || LEDAlt == 0xff)
|
||||
do {
|
||||
P1 = 0;
|
||||
if (HBXSendCommand(GetLEDI, AzMotor))
|
||||
axis[AzMotor].HBXLEDI = HBXGetByte(AzMotor);
|
||||
if ((axis[AzMotor].HBXLEDI != 0) && (axis[AzMotor].HBXLEDI != 0xFF))
|
||||
P1 += 1;
|
||||
if (HBXSendCommand(GetLEDI, AltMotor))
|
||||
axis[AltMotor].HBXLEDI = HBXGetByte(AltMotor);
|
||||
if ((axis[AltMotor].HBXLEDI != 0) && (axis[AltMotor].HBXLEDI != 0xFF))
|
||||
P1 += 1;
|
||||
TimerDelaymS(MOTORDETECT); // Wait .5s between loops
|
||||
} while (P1 < 2);
|
||||
}
|
||||
|
||||
void AzInitialise(void) {
|
||||
// ETX
|
||||
axis[AzMotor].HBXP1 = 0x00;
|
||||
axis[AzMotor].HBXP2 = 0x00;
|
||||
axis[AzMotor].HBXP3 = 0x00;
|
||||
axis[AzMotor].HBXP4 = 0x00;
|
||||
|
||||
axis[AzMotor].HBXPosn = ETX_CENTRE;
|
||||
axis[AzMotor].HBXTarget = axis[AzMotor].HBXPosn;
|
||||
axis[AzMotor].HBXDirSpeed = 0x000;
|
||||
axis[AzMotor].HBXMotorStatus = MOVESTEP;
|
||||
|
||||
axis[AzMotor].HBX_bVALUE = EQG_RAbVALUE;
|
||||
axis[AzMotor].SIDEREALRATE = AzSIDEREALRATE;
|
||||
axis[AzMotor].SOLARRATE = AzSOLARRATE;
|
||||
axis[AzMotor].LUNARRATE = AzLUNARRATE;
|
||||
axis[AzMotor].DEGREERATE1 = AzDEGREERATE1;
|
||||
axis[AzMotor].DEGREERATE2 = AzDEGREERATE2;
|
||||
axis[AzMotor].SIDEREALSPEED = AzSIDEREALSPEED;
|
||||
axis[AzMotor].SOLARSPEED = AzSOLARSPEED;
|
||||
axis[AzMotor].LUNARSPEED = AzLUNARSPEED;
|
||||
axis[AzMotor].DEGREESPEED1 = AzDEGREESPEED1;
|
||||
axis[AzMotor].DEGREESPEED2 = AzDEGREESPEED2;
|
||||
axis[AzMotor].STEPSPER360 = AzSTEPSPER360;
|
||||
axis[AzMotor].WORM = AzWORM;
|
||||
axis[AzMotor].PEC = AzPEC;
|
||||
|
||||
axis[AzMotor].ETXMotorState = ETXCheckStartup;
|
||||
}
|
||||
|
||||
void AltInitialise(void) {
|
||||
// ETX
|
||||
axis[AltMotor].HBXP1 = 0x00;
|
||||
axis[AltMotor].HBXP2 = 0x00;
|
||||
axis[AltMotor].HBXP3 = 0x00;
|
||||
axis[AltMotor].HBXP4 = 0x00;
|
||||
|
||||
axis[AltMotor].HBXPosn = ETX_CENTRE;
|
||||
axis[AltMotor].HBXTarget = axis[AltMotor].HBXPosn;
|
||||
axis[AltMotor].HBXDirSpeed = 0x000;
|
||||
axis[AltMotor].HBXMotorStatus = MOVESTEP;
|
||||
|
||||
axis[AltMotor].HBX_bVALUE = EQG_DECbVALUE;
|
||||
axis[AltMotor].SIDEREALRATE = AltSIDEREALRATE;
|
||||
axis[AltMotor].SOLARRATE = AltSOLARRATE;
|
||||
axis[AltMotor].LUNARRATE = AltLUNARRATE;
|
||||
axis[AltMotor].DEGREERATE1 = AltDEGREERATE1;
|
||||
axis[AltMotor].DEGREERATE2 = AltDEGREERATE2;
|
||||
axis[AltMotor].SIDEREALSPEED = AltSIDEREALSPEED;
|
||||
axis[AltMotor].SOLARSPEED = AltSOLARSPEED;
|
||||
axis[AltMotor].LUNARSPEED = AltLUNARSPEED;
|
||||
axis[AltMotor].DEGREESPEED1 = AltDEGREESPEED1;
|
||||
axis[AltMotor].DEGREESPEED2 = AltDEGREESPEED2;
|
||||
axis[AltMotor].STEPSPER360 = AltSTEPSPER360;
|
||||
axis[AltMotor].WORM = AltWORM;
|
||||
axis[AltMotor].PEC = AltPEC;
|
||||
|
||||
axis[AltMotor].ETXMotorState = ETXCheckStartup;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
/*
|
||||
* Copyright 2017, 2018 John Archbold
|
||||
*/
|
||||
|
||||
|
||||
/********************************************************
|
||||
EQG Protocol function definitions
|
||||
=================================
|
||||
*********************************************************/
|
||||
#ifndef HBXComms
|
||||
#define HBXComms
|
||||
|
||||
// Pin definitions for HBX interface
|
||||
// =================================
|
||||
#define HDA1 8 // Pin2, 4, 6 on HBX interface
|
||||
#define HCL1 2 // Pin3 on HBX interface
|
||||
#define HDA2 10 // Not used
|
||||
#define HCL2 3 // Pin5 on HBX interface
|
||||
|
||||
#define CR 0x0d
|
||||
#define LF 0x0a
|
||||
|
||||
#define HBXLEN 16
|
||||
#define HBXMASK HBXLEN-1
|
||||
|
||||
#define H2XRESETTIME 25 // Reset H2X bus
|
||||
#define BITTIME 120 // H2X clock ~200uS i.e 100us Low/High
|
||||
#define LOWTIME 120 // H2X clock ~200uS i.e 100us Low/High
|
||||
#define DSTABLE 60 // H2X data write delay ~ 50uS
|
||||
#define DGLITCH 5 // H2X data glitch sample
|
||||
#define HIGHTIME 120 // H2X clock ~200uS i.e 100us Low/High
|
||||
#define ETXDELAY 6.55 // H2X ETX poll delay (mS)
|
||||
#define CMNDTIME 1 // H2X command delay (mS)
|
||||
#define STARTTIME 50 // H2X startup time for motors
|
||||
#define CLOCKTIMEOUT 50 // H2X Clock transition timeout (uS) (for monitor mode)
|
||||
#define MOTORDETECT 500 // H2X Detect Motor controller
|
||||
|
||||
void TimerDelayuS(unsigned int);
|
||||
|
||||
bool HBXSendCommand(unsigned char, unsigned char);
|
||||
bool HBXStartSequence(unsigned char);
|
||||
void HBXSendByte(unsigned char, unsigned char);
|
||||
unsigned char HBXGetByte(unsigned char);
|
||||
void HBXSend2Bytes(unsigned char);
|
||||
void HBXSend3Bytes(unsigned char);
|
||||
void HBXGet3Bytes(unsigned char);
|
||||
|
||||
void HBXTestLoop(void);
|
||||
void HBXMonitorMode(void);
|
||||
bool HBXMonitorHCL(unsigned char);
|
||||
bool HBXMonitorBit(unsigned char);
|
||||
bool HBXMonitorByte(unsigned char);
|
||||
void HBXMonitorEnd(unsigned char);
|
||||
void HBXMonitorMessage(unsigned char);
|
||||
void HBXSaveState(unsigned char);
|
||||
void HBXPrintStatus(unsigned char);
|
||||
void HBXPrintState(unsigned char);
|
||||
|
||||
void HDAListen(void);
|
||||
void HDATalk(void);
|
||||
void HCL1Listen(void);
|
||||
void HCL1Talk(void);
|
||||
void HCL2Listen(void);
|
||||
void HCL2Talk(void);
|
||||
void H2XReset(void);
|
||||
bool ResetMotor(unsigned char);
|
||||
|
||||
long TwosComplement(long);
|
||||
|
||||
// Monitor
|
||||
void HBXMonitorLoop(void);
|
||||
|
||||
// Testing
|
||||
void HBXTest(void);
|
||||
bool HBXGet2Status(void);
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,243 @@
|
|||
/*
|
||||
* Copyright 2017, 2018 John Archbold
|
||||
*/
|
||||
|
||||
|
||||
/********************************************************
|
||||
HBX Comms related functions
|
||||
===========================
|
||||
*********************************************************/
|
||||
|
||||
// HBX transmission functions
|
||||
// ==========================
|
||||
|
||||
// HBX Send a command
|
||||
// ------------------
|
||||
bool HBXSendCommand(unsigned char Command, unsigned char Motor) {
|
||||
|
||||
unsigned long i;
|
||||
axis[Motor].HBXCmnd = Command;
|
||||
|
||||
// Send the start sequence
|
||||
// -----------------------
|
||||
if (HBXStartSequence(Motor)) {
|
||||
|
||||
// Send the command byte
|
||||
// ---------------------
|
||||
HBXSendByte(Command, Motor);
|
||||
return(true);
|
||||
}
|
||||
else return(false);
|
||||
}
|
||||
|
||||
// HBX Initiate start sequence
|
||||
// ---------------------------
|
||||
bool HBXStartSequence(unsigned char Motor) {
|
||||
|
||||
HDAListen(); // HDA as input
|
||||
|
||||
// Set clock low
|
||||
if (Motor == MotorAz) digitalWrite(HCL1, LOW);
|
||||
else digitalWrite(HCL2, LOW);
|
||||
TimerDelayuS(DSTABLE); // Wait for answer
|
||||
|
||||
// Wait for data low by MC, or timeout
|
||||
H2XStart = micros(); // Get the start microseconds
|
||||
do { // Wait for MC to answer with HDA1 = 0
|
||||
H2XTimer = micros() - H2XStart;
|
||||
} while ((digitalRead(HDA1) == 1) && (H2XTimer < (BITTIME << 3)));
|
||||
TimerDelayuS(DGLITCH); // Just in case of data line glitch
|
||||
|
||||
// Check if (data low transition) or ( MC timeout)
|
||||
if ((digitalRead(HDA1) == 1) || (H2XTimer >= (BITTIME << 3))) {
|
||||
if (Motor == MotorAz) digitalWrite(HCL1, HIGH);
|
||||
else digitalWrite(HCL2, HIGH);
|
||||
return(false); // Error Exit if no response from Motor
|
||||
}
|
||||
|
||||
// Set clock high if data low transition (i.e. MC acknowledged clock)
|
||||
if (Motor == MotorAz) digitalWrite(HCL1, HIGH);
|
||||
else digitalWrite(HCL2, HIGH);
|
||||
TimerDelayuS(DSTABLE);
|
||||
|
||||
// Wait for data line release by MC, or timeout
|
||||
H2XStart = micros(); // Get the start microseconds
|
||||
do { // Wait for MC to answer
|
||||
H2XTimer = micros() - H2XStart;
|
||||
} while ((digitalRead(HDA1) == 0) && (H2XTimer < (BITTIME << 3)));
|
||||
TimerDelayuS(BITTIME);
|
||||
|
||||
// Check timeout for data line released
|
||||
if (H2XTimer >= 8*BITTIME) {
|
||||
return(false); // Error Exit if no response from Motor
|
||||
}
|
||||
return(true); // Success
|
||||
}
|
||||
|
||||
// HBX Send a single byte
|
||||
// ----------------------
|
||||
void HBXSendByte(unsigned char databyte, unsigned char Motor) {
|
||||
|
||||
unsigned char b, mask;
|
||||
|
||||
HDATalk(); // HDA as output
|
||||
axis[Motor].HBXBitCount = 8; // 8bits to go
|
||||
mask = 0x80; // MSB first
|
||||
|
||||
// Clock was set high before entry
|
||||
TimerDelayuS(HIGHTIME);
|
||||
do {
|
||||
axis[Motor].HBXBitCount -= 1;
|
||||
|
||||
// Set data bit
|
||||
if (databyte & mask) digitalWrite(HDA1, HIGH);
|
||||
else digitalWrite(HDA1, LOW);
|
||||
TimerDelayuS(DSTABLE); // Let data stabilise
|
||||
mask = mask >> 1; // Next data bit
|
||||
|
||||
// Set clock low
|
||||
if (Motor == MotorAz) digitalWrite(HCL1, LOW);
|
||||
else digitalWrite(HCL2, LOW);
|
||||
TimerDelayuS(LOWTIME);
|
||||
|
||||
if (!(axis[Motor].HBXBitCount)) { // Last bit -> force float on data
|
||||
digitalWrite(HDA1, LOW);
|
||||
HDAListen();
|
||||
}
|
||||
|
||||
// Set clock high
|
||||
if (Motor == MotorAz) digitalWrite(HCL1, HIGH);
|
||||
else digitalWrite(HCL2, HIGH);
|
||||
TimerDelayuS(HIGHTIME-DSTABLE); // Data is written DSTABLE before clock low
|
||||
|
||||
// for 8 bits
|
||||
} while (axis[Motor].HBXBitCount);
|
||||
|
||||
TimerDelayuS(DSTABLE); // Last high clock
|
||||
}
|
||||
|
||||
// HBX Send two bytes in sequence
|
||||
// ------------------------------
|
||||
void HBXSend2Bytes(unsigned char Motor) {
|
||||
HBXSendByte(axis[Motor].HBXP1, Motor);
|
||||
HBXSendByte(axis[Motor].HBXP2, Motor);
|
||||
}
|
||||
|
||||
// HBX Send three bytes in sequence
|
||||
// --------------------------------
|
||||
void HBXSend3Bytes(unsigned char Motor) {
|
||||
HBXSendByte(axis[Motor].HBXP1, Motor);
|
||||
HBXSendByte(axis[Motor].HBXP2, Motor);
|
||||
HBXSendByte(axis[Motor].HBXP3, Motor);
|
||||
}
|
||||
|
||||
// HBX Get a single byte
|
||||
// ----------------------
|
||||
unsigned char HBXGetByte(unsigned char Motor) {
|
||||
|
||||
unsigned long i;
|
||||
unsigned char b;
|
||||
|
||||
HDAListen(); // HDA as input
|
||||
axis[Motor].HBXBitCount = 8;
|
||||
axis[Motor].HBXData = 0;
|
||||
|
||||
// Clock was set high before entry
|
||||
while (axis[Motor].HBXBitCount) {
|
||||
|
||||
// Set clock low
|
||||
if (Motor == MotorAz) digitalWrite(HCL1, LOW);
|
||||
else digitalWrite(HCL2, LOW);
|
||||
TimerDelayuS(DSTABLE);
|
||||
|
||||
// Read data bit
|
||||
axis[Motor].HBXData = axis[Motor].HBXData << 1; // Shift previous bit
|
||||
if (digitalRead(HDA1)) axis[Motor].HBXData |= 0x01; // Read next bit
|
||||
axis[Motor].HBXBitCount--; // Need eight bits
|
||||
TimerDelayuS(LOWTIME-DSTABLE); // Wait for low time
|
||||
|
||||
// Set clock high
|
||||
if (Motor == MotorAz) digitalWrite(HCL1, HIGH);
|
||||
else digitalWrite(HCL2, HIGH);
|
||||
TimerDelayuS(HIGHTIME);
|
||||
}
|
||||
|
||||
|
||||
// Return data byte
|
||||
axis[Motor].HBXCount = 1;
|
||||
return (axis[Motor].HBXData);
|
||||
}
|
||||
|
||||
// HBX Get the status bytes (25 bits)
|
||||
// ----------------------------------
|
||||
void HBXGet3Bytes(unsigned char Motor) {
|
||||
unsigned char b;
|
||||
|
||||
axis[Motor].HBXP1 = HBXGetByte(Motor);
|
||||
TimerDelayuS(HIGHTIME);
|
||||
axis[Motor].HBXP2 = HBXGetByte(Motor);
|
||||
TimerDelayuS(HIGHTIME);
|
||||
axis[Motor].HBXP3 = HBXGetByte(Motor);
|
||||
TimerDelayuS(HIGHTIME);
|
||||
axis[Motor].HBXP4 = 0;
|
||||
|
||||
// Read 'byte4' = error bit
|
||||
// ------------------------
|
||||
if (Motor == MotorAz) digitalWrite(HCL1, LOW);
|
||||
else digitalWrite(HCL2, LOW);
|
||||
TimerDelayuS(DSTABLE);
|
||||
axis[Motor].HBXP4 |= digitalRead(HDA1); // Read the error bit
|
||||
TimerDelayuS(LOWTIME-DSTABLE);
|
||||
if (Motor == MotorAz)digitalWrite(HCL1, HIGH);
|
||||
else digitalWrite(HCL2, HIGH);
|
||||
TimerDelayuS(HIGHTIME);
|
||||
|
||||
axis[Motor].HBXCount = 4;
|
||||
}
|
||||
|
||||
// H2X Low level Functions
|
||||
// -----------------------
|
||||
void HDAListen() {
|
||||
pinMode(HDA1, H2X_INPUT);
|
||||
// digitalWrite(HDA1, HIGH);
|
||||
}
|
||||
void HDAFloat() {
|
||||
pinMode(HDA1, H2X_INPUT);
|
||||
}
|
||||
void HDATalk() {
|
||||
digitalWrite(HDA1, HIGH);
|
||||
pinMode(HDA1, H2X_OUTPUT);
|
||||
}
|
||||
void HCL1Listen() {
|
||||
pinMode(HCL1, H2X_INPUT);
|
||||
}
|
||||
void HCL1Talk() {
|
||||
digitalWrite(HCL1, HIGH);
|
||||
pinMode(HCL1, H2X_OUTPUT);
|
||||
}
|
||||
void HCL2Listen() {
|
||||
pinMode(HCL2, H2X_INPUT);
|
||||
}
|
||||
void HCL2Talk() {
|
||||
digitalWrite(HCL2, HIGH);
|
||||
pinMode(HCL2, H2X_OUTPUT);
|
||||
}
|
||||
|
||||
void H2XReset() {
|
||||
HCL1Talk();
|
||||
HCL2Talk();
|
||||
HDATalk();
|
||||
digitalWrite(HDA1, LOW);
|
||||
TimerDelayuS(H2XRESETTIME);
|
||||
digitalWrite(HDA1, HIGH);
|
||||
TimerDelayuS(H2XRESETTIME);
|
||||
HDAListen();
|
||||
}
|
||||
|
||||
long TwosComplement(long p) { // Calculate 2s complement
|
||||
long q;
|
||||
q = ~p; // Bitwise invert
|
||||
q = q + 1; // +1
|
||||
return q;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,242 @@
|
|||
/*
|
||||
* Copyright 2017, 2018 John Archbold
|
||||
*/
|
||||
|
||||
|
||||
/********************************************************
|
||||
Utility functions to monitor HBX comms
|
||||
======================================
|
||||
|
||||
ToDo:
|
||||
Change to H2XISR for interrupt driven receive
|
||||
*********************************************************/
|
||||
void HBXMonitorLoop(void){
|
||||
int Messages;
|
||||
PreviousTime = millis();
|
||||
|
||||
Serial.println("ETX-Monitor");
|
||||
// H2XReset();
|
||||
axis[AzMotor].PrintStatus0 = 1; // Enable print of status = no change
|
||||
axis[AltMotor].PrintStatus0 = 1; // Enable print of status = no change
|
||||
HBXMonitorMode();
|
||||
|
||||
do {
|
||||
Messages = 0;
|
||||
if (HBXMonitorHCL(CLOCKTIMEOUT)) {
|
||||
if (DetectedClock) {
|
||||
HBXMonitorMessage(DetectedClock); // Wait for low clock and print data
|
||||
axis[DetectedClock].TimeDelta = millis(); // - PreviousTime;
|
||||
Messages += 1;
|
||||
}
|
||||
}
|
||||
if (HBXMonitorHCL(CLOCKTIMEOUT)) { // Better read Alt
|
||||
if (DetectedClock) {
|
||||
HBXMonitorMessage(DetectedClock); // Alt may follow immediately
|
||||
axis[DetectedClock].TimeDelta = millis(); // - PreviousTime;
|
||||
Messages += 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (Messages) {
|
||||
switch(Messages) {
|
||||
case 1:
|
||||
HBXPrintState(DetectedClock);
|
||||
break;
|
||||
case 2:
|
||||
if (DetectedClock == MotorAz) {
|
||||
HBXPrintState(MotorAlt);
|
||||
HBXPrintState(MotorAz);
|
||||
}
|
||||
else {
|
||||
HBXPrintState(MotorAz);
|
||||
HBXPrintState(MotorAlt);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
} while (digitalRead(MONITORHBX) == 0); // Check if jumper installed
|
||||
}
|
||||
|
||||
void HBXMonitorMode(void) {
|
||||
|
||||
HDAListen(); // HDA as input
|
||||
HCL1Listen(); // HCL as input
|
||||
HCL2Listen();
|
||||
TimerDelaymS(STARTTIME);
|
||||
}
|
||||
|
||||
bool HBXMonitorHCL(unsigned char Timeout) {
|
||||
int j;
|
||||
DetectedClock = 0;
|
||||
j = Timeout;
|
||||
while (!digitalRead(HDA1)); // Wait for data high
|
||||
while (DetectedClock == 0) { // Wait for a low on either clock
|
||||
if (digitalRead(HCL1) == 0) {
|
||||
TimerDelayuS(3); // Re-read in case of glitches
|
||||
if (digitalRead(HCL1) == 0) DetectedClock = MotorAz;
|
||||
if (digitalRead(HCL2) == 0) { // Fail if both low
|
||||
DetectedClock = 0;
|
||||
return (false);
|
||||
}
|
||||
}
|
||||
else if (digitalRead(HCL2) == 0) {
|
||||
TimerDelayuS(3); // Re-read in case of glitches
|
||||
if (digitalRead(HCL2) == 0) DetectedClock = MotorAlt;
|
||||
if (digitalRead(HCL1) == 0) { // Fail if both low
|
||||
DetectedClock = 0;
|
||||
return (false);
|
||||
}
|
||||
}
|
||||
// j -= 1;
|
||||
}
|
||||
|
||||
// if (j) {
|
||||
while (digitalRead(HDA1)); // Wait for data low
|
||||
while (digitalRead(HDA1)); // Wait for data low again
|
||||
if (DetectedClock == MotorAz) { // Check clock is still low
|
||||
if (digitalRead(HCL1)) return (false);
|
||||
}
|
||||
else if (digitalRead(HCL2)) return (false);
|
||||
|
||||
if (DetectedClock == MotorAz) {
|
||||
while (!digitalRead(HCL1)); // Wait for clock high
|
||||
}
|
||||
else while (!digitalRead(HCL2));
|
||||
return (true);
|
||||
// }
|
||||
// return (false); // Timeout
|
||||
}
|
||||
|
||||
void HBXMonitorEnd(unsigned char Motor) {
|
||||
|
||||
if (Motor == MotorAz) while (!digitalRead(HCL1)); // Wait for clock high
|
||||
else while (!digitalRead(HCL2));
|
||||
}
|
||||
|
||||
bool HBXMonitorBit(unsigned char Motor) {
|
||||
// Serial.write('!');
|
||||
axis[Motor].HBXData = 0;
|
||||
if (Motor == MotorAz) while (!digitalRead(HCL1)); // Wait for clock high
|
||||
else while (!digitalRead(HCL2));
|
||||
if (Motor == MotorAz) while (digitalRead(HCL1)); // Wait for clock low
|
||||
else while (digitalRead(HCL2));
|
||||
if (digitalRead(HDA1)) axis[Motor].HBXData |= 0x01; // Read the bit
|
||||
return(true);
|
||||
}
|
||||
|
||||
bool HBXMonitorByte(unsigned char Motor) {
|
||||
// Serial.write('>');
|
||||
axis[Motor].HBXBitCount = 8;
|
||||
axis[Motor].HBXData = 0;
|
||||
while (axis[Motor].HBXBitCount) {
|
||||
axis[Motor].HBXData = axis[Motor].HBXData << 1; // Shift previous bit
|
||||
axis[Motor].HBXBitCount--; // Need eight bits
|
||||
if (Motor == MotorAz) while (!digitalRead(HCL1)); // Wait for clock high
|
||||
else while (!digitalRead(HCL2));
|
||||
if (Motor == MotorAz) while (digitalRead(HCL1)); // Wait for clock low
|
||||
else while (digitalRead(HCL2));
|
||||
if (digitalRead(HDA1)) axis[Motor].HBXData |= 0x01; // Read next bit
|
||||
}
|
||||
return(true);
|
||||
}
|
||||
|
||||
void HBXMonitorMessage(unsigned char Motor) {
|
||||
// Serial.write('<');
|
||||
if (HBXMonitorByte(Motor))
|
||||
axis[Motor].HBXCmnd = axis[Motor].HBXData;
|
||||
|
||||
switch (axis[Motor].HBXCmnd) {
|
||||
|
||||
case GetStatus: // Four bytes of data
|
||||
axis[Motor].HBXCount = 4;
|
||||
if (HBXMonitorByte(Motor)) axis[Motor].HBXP1 = axis[Motor].HBXData;
|
||||
if (HBXMonitorByte(Motor)) axis[Motor].HBXP2 = axis[Motor].HBXData;
|
||||
if (HBXMonitorByte(Motor)) axis[Motor].HBXP3 = axis[Motor].HBXData;
|
||||
if (HBXMonitorBit(Motor)) axis[Motor].HBXP4 = axis[Motor].HBXData;
|
||||
break;
|
||||
|
||||
case RotateSlow: // Three bytes of data
|
||||
case RotateFast:
|
||||
axis[Motor].HBXCount = 3;
|
||||
if (HBXMonitorByte(Motor)) axis[Motor].HBXP1 = axis[Motor].HBXData;
|
||||
if (HBXMonitorByte(Motor)) axis[Motor].HBXP2 = axis[Motor].HBXData;
|
||||
if (HBXMonitorByte(Motor)) axis[Motor].HBXP3 = axis[Motor].HBXData;
|
||||
break;
|
||||
|
||||
case SetOffset: // Two bytes of data
|
||||
axis[Motor].HBXCount = 2;
|
||||
if (HBXMonitorByte(Motor)) axis[Motor].HBXP1 = axis[Motor].HBXData;
|
||||
if (HBXMonitorByte(Motor)) axis[Motor].HBXP2 = axis[Motor].HBXData;
|
||||
break;
|
||||
|
||||
case SetLEDI: // One byte of data
|
||||
case GetLEDI:
|
||||
case GetMotorType:
|
||||
axis[Motor].HBXCount = 1;
|
||||
if (HBXMonitorByte(Motor)) axis[Motor].HBXP1 = axis[Motor].HBXData;
|
||||
break;
|
||||
|
||||
case CalibrateLED: // No data
|
||||
case Stop:
|
||||
case SlewReverse:
|
||||
case SlewForward:
|
||||
case ResetH2X:
|
||||
axis[Motor].HBXCount = 0;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if (Motor == MotorAz) while (!digitalRead(HCL1)); // Wait for clock high
|
||||
else while (!digitalRead(HCL2));
|
||||
}
|
||||
|
||||
void HBXPrintState(unsigned char Motor) {
|
||||
if (axis[Motor].HBXCmnd != GetStatus) { // Handle all other commands
|
||||
Serial.print(Motor);
|
||||
Serial.write(',');
|
||||
Serial.print(axis[Motor].TimeDelta);
|
||||
Serial.write(',');
|
||||
Serial.print(axis[Motor].HBXCmnd, HEX);
|
||||
if (axis[Motor].HBXCount) {
|
||||
Serial.write(',');
|
||||
Serial.print(axis[Motor].HBXP1);
|
||||
axis[Motor].HBXCount -= 1;
|
||||
}
|
||||
if (axis[Motor].HBXCount) {
|
||||
Serial.write(',');
|
||||
Serial.print(axis[Motor].HBXP2);
|
||||
axis[Motor].HBXCount -= 1;
|
||||
}
|
||||
if (axis[Motor].HBXCount) {
|
||||
Serial.write(',');
|
||||
Serial.print(axis[Motor].HBXP3);
|
||||
axis[Motor].HBXCount -= 1;
|
||||
}
|
||||
if (axis[Motor].HBXCount) {
|
||||
Serial.write(',');
|
||||
Serial.print(axis[Motor].HBXP4, HEX);
|
||||
}
|
||||
}
|
||||
|
||||
// Handle report status - exclude all 0 - ie nothing happened
|
||||
else if (axis[Motor].HBXP1 | axis[Motor].HBXP2 | axis[Motor].HBXP3 | axis[Motor].HBXP4 | axis[Motor].PrintStatus0) {
|
||||
Serial.print(Motor);
|
||||
Serial.write(',');
|
||||
Serial.print(axis[Motor].TimeDelta);
|
||||
Serial.write(',');
|
||||
Serial.print(axis[Motor].HBXCmnd);
|
||||
Serial.write(',');
|
||||
Serial.print((axis[Motor].HBXP1<<8) + axis[Motor].HBXP2);
|
||||
Serial.write(',');
|
||||
Serial.print(axis[Motor].HBXP3);
|
||||
Serial.write(',');
|
||||
Serial.print(axis[Motor].HBXP4, HEX);
|
||||
}
|
||||
Serial.println("");
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,439 @@
|
|||
/*
|
||||
* Copyright 2017, 2018 John Archbold
|
||||
*/
|
||||
|
||||
|
||||
/********************************************************
|
||||
Test HBX communications
|
||||
=======================
|
||||
*********************************************************/
|
||||
void HBXTestLoop(void) {
|
||||
TestCount = 0;
|
||||
while (digitalRead(TESTHBX) == 0) {
|
||||
Serial.println("Test HBX commands to ETX");
|
||||
HBXTest();
|
||||
TestCount += 1;
|
||||
TestLoopTime = millis();
|
||||
// Read motor status until jumper removed
|
||||
while((millis() - TestLoopTime) < 5000) { // 5s between tests
|
||||
HBXGet2Status();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void HBXTest(void)
|
||||
{
|
||||
int i;
|
||||
|
||||
// Initialize HBX communications as outputs
|
||||
// It will use I2C-like communications
|
||||
Serial.println("**********************************************");
|
||||
Serial.print("Test Number - ");
|
||||
Serial.println(TestCount);
|
||||
Serial.println("**********************************************");
|
||||
//do {
|
||||
digitalWrite(FROMHBX, HIGH);
|
||||
Serial1.println("**********************************************");
|
||||
// TimerDelaymS(100);
|
||||
Serial1.print("Test Number - ");
|
||||
// TimerDelaymS(100);
|
||||
Serial1.println(TestCount++);
|
||||
digitalWrite(FROMHBX, LOW);
|
||||
|
||||
//} while(1);
|
||||
|
||||
HCL1Talk(); // Set for Talking on RAClk
|
||||
HCL2Talk(); // Set for Talking on DECClk
|
||||
HDAListen();
|
||||
TimerDelaymS(STARTTIME);
|
||||
axis[AzMotor].PrintStatus0 = 1; // Enable print of status = no change
|
||||
axis[AltMotor].PrintStatus0 = 1; // Enable print of status = no change
|
||||
|
||||
//if (TestCount == 0) {
|
||||
|
||||
Serial.println("Test - HBX Initialization");
|
||||
axis[AzMotor].HBXPosn = ETX_CENTRE;
|
||||
axis[AzMotor].HBXTarget = axis[AzMotor].HBXPosn;
|
||||
axis[AzMotor].HBXDirSpeed = 0x000;
|
||||
axis[AzMotor].HBXSpeed = 0x000000;
|
||||
axis[AzMotor].HBXMotorStatus = MOVESTEP;
|
||||
|
||||
axis[AltMotor].HBXPosn = ETX_CENTRE;
|
||||
axis[AltMotor].HBXTarget = axis[AltMotor].HBXPosn;
|
||||
axis[AltMotor].HBXDirSpeed = 0x000;
|
||||
axis[AltMotor].HBXSpeed = 0x000000;
|
||||
axis[AltMotor].HBXMotorStatus = MOVESTEP;
|
||||
|
||||
// Reset the motors (RA and DEC)
|
||||
Serial.println("Test - Wait for motors");
|
||||
|
||||
// GetLED commands always return a vaild value - motors not online until this happens
|
||||
// Valid values are not 0 and not 0xFF for Az, Alt. (exception here is if LEDRA || LEDAlt == 0xff)
|
||||
|
||||
do {
|
||||
P1 = 0;
|
||||
if (HBXSendCommand(GetLEDI, AzMotor))
|
||||
P2 = HBXGetByte(AzMotor);
|
||||
if ((P2 != 0) && (P2 != 0xFF)) P1 += 1;
|
||||
TimerDelaymS(CMNDTIME);
|
||||
if (HBXSendCommand(GetLEDI, AltMotor))
|
||||
P2 = HBXGetByte(AltMotor);
|
||||
if ((P2 != 0) && (P2 != 0xFF)) P1 += 1;
|
||||
TimerDelaymS(CMNDTIME);
|
||||
} while (P1 < 2);
|
||||
|
||||
// Stop the motors (Az and Alt)
|
||||
Serial.println("Test - Stop motors");
|
||||
HBXStop2Motors();
|
||||
|
||||
// Read status
|
||||
Serial.println("Test - Read Status");
|
||||
HBXGet2Status(); // Check and read both motor states
|
||||
|
||||
|
||||
|
||||
if (!check_eeprom_crc() || !EEPROM.read(EEPROMAzLEDI) || !EEPROM.read(EEPROMAltLEDI)) {
|
||||
// Calibrate Az, Alt Motor Encoder LED currents
|
||||
Serial.println("Test - Calibrate RA, DEC Motor Encoder LED currents");
|
||||
if (HBXSendCommand(CalibrateLED, AzMotor));
|
||||
TimerDelaymS(2500);
|
||||
HBXPrintStatus(AzMotor);
|
||||
|
||||
if (HBXSendCommand(CalibrateLED, AltMotor))
|
||||
TimerDelaymS(2500);
|
||||
HBXPrintStatus(AltMotor);
|
||||
|
||||
if (HBXSendCommand(GetLEDI, AzMotor))
|
||||
axis[AzMotor].HBXLEDI = HBXGetByte(AzMotor);
|
||||
EEPROM.write(EEPROMAzLEDI, axis[AzMotor].HBXLEDI);
|
||||
set_eeprom_crc();
|
||||
axis[AzMotor].HBXP1 = axis[AzMotor].HBXLEDI;
|
||||
HBXPrintStatus(AzMotor);
|
||||
if (HBXSendCommand(GetLEDI, AltMotor))
|
||||
axis[AltMotor].HBXLEDI = HBXGetByte(AltMotor);
|
||||
EEPROM.write(EEPROMAltLEDI, axis[AltMotor].HBXLEDI);
|
||||
set_eeprom_crc();
|
||||
axis[AltMotor].HBXP1 = axis[AltMotor].HBXLEDI;
|
||||
HBXPrintStatus(AltMotor);
|
||||
}
|
||||
|
||||
Serial.println("Test - Get Motor Type and set ETX Encoder LED currents");
|
||||
axis[AzMotor].MotorType = 0x00;
|
||||
while (!axis[AzMotor].MotorType) {
|
||||
if (HBXSendCommand(GetMotorType, AzMotor))
|
||||
axis[AzMotor].MotorType = HBXGetByte(AzMotor);
|
||||
HBXPrintStatus(AzMotor);
|
||||
}
|
||||
|
||||
axis[AzMotor].HBXLEDI = EEPROM.read(EEPROMAzLEDI);
|
||||
axis[AltMotor].HBXLEDI = EEPROM.read(EEPROMAltLEDI);
|
||||
|
||||
if (HBXSendCommand(SetLEDI, AzMotor))
|
||||
HBXSendByte(axis[AzMotor].HBXLEDI, AzMotor);
|
||||
axis[AzMotor].HBXP1 = axis[AzMotor].HBXLEDI;
|
||||
HBXPrintStatus(AzMotor);
|
||||
|
||||
if (HBXSendCommand(SetLEDI, AltMotor))
|
||||
HBXSendByte(axis[AltMotor].HBXLEDI, AltMotor);
|
||||
axis[AltMotor].HBXP1 = axis[AltMotor].HBXLEDI;
|
||||
HBXPrintStatus(AltMotor);
|
||||
|
||||
// Set the Offset to Zero
|
||||
axis[AzMotor].HBXP1 = 0x00;
|
||||
axis[AzMotor].HBXP2 = 0x00;
|
||||
axis[AzMotor].HBXP3 = 0x00;
|
||||
axis[AltMotor].HBXP1 = 0x00;
|
||||
axis[AltMotor].HBXP2 = 0x00;
|
||||
axis[AltMotor].HBXP3 = 0x00;
|
||||
|
||||
// Set the Offset Clear Command
|
||||
Serial.println("Test - Reset any ETX offset bytes");
|
||||
if (HBXSendCommand(SetOffset, AzMotor))
|
||||
HBXSend2Bytes(AzMotor);
|
||||
// HBXPrintStatus(AzMotor);
|
||||
TimerDelaymS(CMNDTIME);
|
||||
|
||||
if (HBXSendCommand(SetOffset, AltMotor))
|
||||
HBXSend2Bytes(AltMotor);
|
||||
// HBXPrintStatus(AltMotor);
|
||||
TimerDelaymS(CMNDTIME);
|
||||
|
||||
//}
|
||||
|
||||
Serial.println("Test - Stop motors");
|
||||
HBXStop2Motors();
|
||||
// First Read, clear counters
|
||||
Serial.println("Test - Read Both Motor States");
|
||||
HBXGet2Status();
|
||||
|
||||
Serial.println("Test - Begin motor move tests");
|
||||
//}
|
||||
// Test different motor speeds
|
||||
Serial.println("Test - Motor Speed Tests");
|
||||
Serial.println("========================");
|
||||
|
||||
Serial.println("Test - SIDEREAL");
|
||||
axis[AzMotor].HBXMotorStatus |= MOVEDIRN; // Forward
|
||||
axis[AltMotor].HBXMotorStatus |= MOVEDIRN;
|
||||
axis[AzMotor].HBXSpeed = AzSIDEREALRATE; // Sidereal
|
||||
axis[AltMotor].HBXSpeed = AltSIDEREALRATE;
|
||||
HBXPrintPosn(5, 1000); // Show location each second
|
||||
Serial.println("Test - SIDEREAL - Stop motors");
|
||||
HBXStop2Motors();
|
||||
|
||||
Serial.println("Test - OneDegree/sec forward");
|
||||
axis[AzMotor].HBXMotorStatus |= MOVEDIRN; // Forward
|
||||
axis[AltMotor].HBXMotorStatus |= MOVEDIRN;
|
||||
axis[AzMotor].HBXSpeed = AzDEGREERATE1; // One degree/sec
|
||||
axis[AltMotor].HBXSpeed = AltDEGREERATE1; // One degree/sec
|
||||
HBXPrintPosn(5, 500); // Show location each tenth of a second
|
||||
Serial.println("Test - OneDegree/sec forward - Stop motors");
|
||||
HBXStop2Motors();
|
||||
|
||||
Serial.println("Test - OneDegree/sec reverse");
|
||||
axis[AzMotor].HBXMotorStatus &= ~MOVEDIRN; // Reverse
|
||||
axis[AltMotor].HBXMotorStatus &= ~MOVEDIRN;
|
||||
axis[AzMotor].HBXSpeed = AzDEGREERATE1; // One degree/sec
|
||||
axis[AltMotor].HBXSpeed = AltDEGREERATE1; // One degree/sec
|
||||
HBXPrintPosn(5, 500); // Show location each each tenth of a second
|
||||
Serial.println("Test - OneDegree/sec reverse - Stop motors");
|
||||
HBXStop2Motors();
|
||||
|
||||
Serial.println("Test - TwoDegrees/sec forward");
|
||||
axis[AzMotor].HBXMotorStatus |= MOVEDIRN; // Forward
|
||||
axis[AltMotor].HBXMotorStatus |= MOVEDIRN;
|
||||
axis[AzMotor].HBXSpeed = AzDEGREERATE1*2; // Two degrees/sec
|
||||
axis[AltMotor].HBXSpeed = AltDEGREERATE1*2; // Two degrees/sec
|
||||
HBXPrintPosn(5, 1000); // Show location each tenth of a second
|
||||
Serial.println("Test - TwoDegrees/sec forward - Stop motors");
|
||||
HBXStop2Motors();
|
||||
|
||||
Serial.println("Test - TwoDegrees/sec reverse");
|
||||
axis[AzMotor].HBXMotorStatus &= ~MOVEDIRN; // Reverse
|
||||
axis[AltMotor].HBXMotorStatus &= ~MOVEDIRN;
|
||||
axis[AzMotor].HBXSpeed = AzDEGREERATE1*2; // Two degrees/sec
|
||||
axis[AltMotor].HBXSpeed = AltDEGREERATE1*2; // Two degrees/sec
|
||||
HBXPrintPosn(5, 1000); // Show location each each tenth of a second
|
||||
Serial.println("Test - TwoDegrees/sec reverse - Stop motors");
|
||||
HBXStop2Motors();
|
||||
|
||||
// Read status
|
||||
Serial.println("Test - Read Status");
|
||||
HBXGet2Status();
|
||||
|
||||
// Stop the motors (Az and Alt)
|
||||
Serial.println("Test - Stop motors");
|
||||
HBXStop2Motors();
|
||||
}
|
||||
|
||||
void HBXPrintPosn(unsigned int count, unsigned int duration) {
|
||||
int j = 0;
|
||||
|
||||
axis[AzMotor].HBXPosn = ETX_CENTRE; // Reset position
|
||||
axis[AltMotor].HBXPosn = ETX_CENTRE;
|
||||
axis[AzMotor].HBXDelta = 0;
|
||||
axis[AltMotor].HBXDelta = 0;
|
||||
PreviousTime = millis();
|
||||
|
||||
HBXStart2Motors(); // Start the motors
|
||||
|
||||
do {
|
||||
ETXState(AzMotor); // Check the Az motor state
|
||||
ETXState(AltMotor); // Check the Alt motor state
|
||||
SendPosn(duration); // Duration is delay between reads
|
||||
j += 1;
|
||||
} while(j < count);
|
||||
}
|
||||
|
||||
void SendPosn(unsigned int duration) {
|
||||
|
||||
// Serial.println(" SendPosn");
|
||||
|
||||
while ((millis() - PreviousTime) < duration) ; // 1/10 second
|
||||
Serial.print(millis() - PreviousTime);
|
||||
Serial.print(" - ");
|
||||
PreviousTime = millis();
|
||||
|
||||
HBXGetStatus(AzMotor);
|
||||
axis[AzMotor].HBXDelta = axis[AzMotor].HBXP1 * 256 + axis[AzMotor].HBXP2;
|
||||
|
||||
HBXGetStatus(AltMotor);
|
||||
axis[AltMotor].HBXDelta = axis[AltMotor].HBXP1 * 256 + axis[AltMotor].HBXP2;
|
||||
|
||||
Serial.print("Az = ");
|
||||
Serial.print(axis[AzMotor].HBXPosn);
|
||||
Serial.print(" : ");
|
||||
Serial.print(axis[AzMotor].HBXDelta);
|
||||
Serial.print(", Alt = ");
|
||||
Serial.print(axis[AltMotor].HBXPosn);
|
||||
Serial.print(" : ");
|
||||
Serial.println(axis[AltMotor].HBXDelta);
|
||||
}
|
||||
|
||||
void HBXPrintStatus(unsigned char Motor) {
|
||||
axis[Motor].HBXCount = 0;
|
||||
if ((axis[Motor].HBXCmnd != GetStatus) || (axis[Motor].HBXP1 | axis[Motor].HBXP2 | axis[Motor].HBXP3 | axis[Motor].HBXP4) || axis[Motor].PrintStatus0 ) {
|
||||
Serial.print("Motor: ");
|
||||
Serial.print(Motor);
|
||||
Serial.print(", Cmnd: ");
|
||||
Serial.print(axis[Motor].HBXCmnd, HEX);
|
||||
Serial.print(" - ");
|
||||
|
||||
switch (axis[Motor].HBXCmnd) {
|
||||
case RotateSlow:
|
||||
Serial.print("RotateSlow ");
|
||||
axis[Motor].HBXCount = 3;
|
||||
break;
|
||||
case RotateFast:
|
||||
Serial.print("RotateFast ");
|
||||
axis[Motor].HBXCount = 3;
|
||||
break;
|
||||
case SetOffset:
|
||||
Serial.print("SetOffset ");
|
||||
axis[Motor].HBXCount = 4;
|
||||
break;
|
||||
case SetLEDI:
|
||||
Serial.print("SetLEDI ");
|
||||
axis[Motor].HBXCount = 1;
|
||||
break;
|
||||
case CalibrateLED:
|
||||
Serial.print("CalibrateLED ");
|
||||
break;
|
||||
case Stop:
|
||||
Serial.print("Stop ");
|
||||
break;
|
||||
case SlewReverse:
|
||||
Serial.print("SlewReverse ");
|
||||
break;
|
||||
case SlewForward:
|
||||
Serial.print("SlewForward ");
|
||||
break;
|
||||
case GetStatus:
|
||||
Serial.print("GetStatus ");
|
||||
axis[Motor].HBXCount = 4;
|
||||
break;
|
||||
case GetLEDI:
|
||||
Serial.print("GetLEDI ");
|
||||
axis[Motor].HBXCount = 1;
|
||||
break;
|
||||
case GetMotorType:
|
||||
Serial.print("GetMotorType ");
|
||||
axis[Motor].HBXP1 = axis[Motor].MotorType;
|
||||
axis[Motor].HBXCount = 1;
|
||||
break;
|
||||
case ResetH2X:
|
||||
Serial.print("ResetH2X ");
|
||||
break;
|
||||
default:
|
||||
Serial.print("UNKNOWN ");
|
||||
break;
|
||||
}
|
||||
|
||||
if (axis[Motor].HBXCount) {
|
||||
Serial.print(", Data: ");
|
||||
Serial.print(axis[Motor].HBXP1, HEX);
|
||||
if (axis[Motor].HBXCount > 1) Serial.print(", ");
|
||||
axis[Motor].HBXCount -= 1;
|
||||
}
|
||||
if (axis[Motor].HBXCount) {
|
||||
Serial.print(axis[Motor].HBXP2, HEX);
|
||||
if (axis[Motor].HBXCount > 1) Serial.print(", ");
|
||||
axis[Motor].HBXCount -= 1;
|
||||
}
|
||||
if (axis[Motor].HBXCount) {
|
||||
Serial.print(axis[Motor].HBXP3, HEX);
|
||||
if (axis[Motor].HBXCount > 1) Serial.print(", ");
|
||||
axis[Motor].HBXCount -= 1;
|
||||
}
|
||||
if (axis[Motor].HBXCount) {
|
||||
Serial.print(axis[Motor].HBXP4, HEX);
|
||||
axis[Motor].HBXCount -= 1;
|
||||
}
|
||||
Serial.println("");
|
||||
}
|
||||
}
|
||||
|
||||
bool HBXStop2Motors(void) {
|
||||
axis[AzMotor].ETXMotorState = ETXStopMotor;
|
||||
ETXState(AzMotor);
|
||||
axis[AltMotor].ETXMotorState = ETXStopMotor;
|
||||
ETXState(AltMotor);
|
||||
return(true);
|
||||
}
|
||||
|
||||
bool HBXStart2Motors(void) {
|
||||
axis[AzMotor].ETXMotorState = ETXCheckStartup;
|
||||
axis[AzMotor].HBXMotorStatus |= MOVEAXIS;
|
||||
ETXState(AzMotor);
|
||||
axis[AltMotor].ETXMotorState = ETXCheckStartup;
|
||||
axis[AltMotor].HBXMotorStatus |= MOVEAXIS;
|
||||
ETXState(AltMotor);
|
||||
return(true);
|
||||
}
|
||||
|
||||
void HBXPrintPosn(unsigned char Motor) {
|
||||
if (Motor == MotorAz) {
|
||||
digitalWrite(FROMHBX, HIGH); // Set Indicator LED
|
||||
Serial.println("");
|
||||
Serial.print("Time: ");
|
||||
Serial.print(millis());
|
||||
// Serial.print(StatusTimer - StatusTime);
|
||||
// StatusTime = StatusTimer;
|
||||
|
||||
|
||||
/* Serial.print(" AzRaw: ");
|
||||
puthexb(axis[AzMotor].HBXP1);
|
||||
putbyte(',');
|
||||
puthexb(axis[AzMotor].HBXP2);
|
||||
putbyte(',');
|
||||
puthexb(axis[AzMotor].HBXP3);
|
||||
putbyte(',');
|
||||
puthexb(axis[AzMotor].HBXP4);
|
||||
*/
|
||||
Serial.print(" AzPosn: ");
|
||||
puthexl(axis[AzMotor].HBXPosn);
|
||||
putbyte(',');
|
||||
puthexl(axis[AzMotor].HBXTarget);
|
||||
putbyte(',');
|
||||
puthexl(axis[AzMotor].HBXSlowDown);
|
||||
Serial.print(" AzSpeed: ");
|
||||
puthexl(axis[AzMotor].HBXSpeed);
|
||||
putbyte(',');
|
||||
puthexl(axis[AzMotor].HBXTargetSpeed);
|
||||
putbyte('-');
|
||||
puthexw(axis[AzMotor].HBXMotorStatus);
|
||||
putbyte(',');
|
||||
puthexw(axis[AzMotor].HBXMotorControl);
|
||||
|
||||
}
|
||||
else {
|
||||
/*
|
||||
* Serial.print(" AltRaw: ");
|
||||
puthexb(axis[AltMotor].HBXP1);
|
||||
putbyte(',');
|
||||
puthexb(axis[AltMotor].HBXP2);
|
||||
putbyte(',');
|
||||
puthexb(axis[AltMotor].HBXP3);
|
||||
putbyte(',');
|
||||
puthexb(axis[AltMotor].HBXP4);
|
||||
*/
|
||||
Serial.print(" AltPosn: ");
|
||||
puthexl(axis[AltMotor].HBXPosn);
|
||||
putbyte(',');
|
||||
puthexl(axis[AltMotor].HBXTarget);
|
||||
putbyte(',');
|
||||
puthexl(axis[AltMotor].HBXSlowDown);
|
||||
Serial.print(" AltSpeed: ");
|
||||
puthexl(axis[AltMotor].HBXSpeed);
|
||||
putbyte(',');
|
||||
puthexl(axis[AltMotor].HBXTargetSpeed);
|
||||
putbyte('-');
|
||||
puthexw(axis[AltMotor].HBXMotorStatus);
|
||||
putbyte(',');
|
||||
puthexw(axis[AltMotor].HBXMotorControl);
|
||||
Serial.println("");
|
||||
}
|
||||
digitalWrite(FROMHBX, LOW); // Clear Indicator LED
|
||||
}
|
||||
|
||||
|
||||
Ładowanie…
Reference in New Issue