mirror
/
EQMOD-ETX
kopia lustrzana https://github.com/ozarchie/EQMOD-ETX
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
EQMOD-ETX/Software/EQG2HBX/EQG2HBXE32/ETXProtocol.ino

905 wiersze
40 KiB
C++
Czysty Wina Historia

/**@file*/
/*
* Copyright 2017, 2018, 2020 John Archbold
*/
#include "Hardware.h"
#include "ETXProtocol.h"
#include "EQGProtocol.h"
#include "EQG2HBX.h"
#include "HBXComms.h"
#include "HBXFileSystem.h"
#include "HBXWiFiServer.h"
/********************************************************
ETX Protocol related functions
==============================
*********************************************************/
bool ETXState(unsigned char Motor) {
long distance;
switch(axis[Motor].ETXMotorState) {
case ETXIdle:
break;
case ETXCheckStartup:
if (axis[Motor].ETXMotorStatus & MOVEAXIS) { // Start moving
//jma CheckAltFlip(Motor);
dbgSerial.println(""); dbgSerial.print("ETXCheckStartup - Motor: "); dbgSerial.print(axis_name[Motor]); dbgSerial.print(" MOVING");
if (axis[Motor].ETXMotorStatus & MOVESLEW) {
axis[Motor].MotorControl |= SlewHBX;
axis[Motor].ETXMotorState = ETXSlewMotor;
if (axis[Motor].ETXMotorStatus & MOVEHIGH) { // Use high speed multiplier?
dbgSerial.print(" HIGH SLEW"); // Ramp up to HIGH SPEED
}
else {
axis[Motor].ETXMotorState = ETXStepMotor;
axis[Motor].MotorControl |= SpeedHBX; // Use 0x01 command for next speed
dbgSerial.print(" LOW SLEW");
}
}
else { // GoTo or Low Speed Slew
axis[Motor].MotorControl &= ~SlewHBX;
axis[Motor].ETXMotorState = ETXCheckSpeed; // so just check speed
dbgSerial.print(" GOTO");
}
if (axis[Motor].MotorControl & GoToHBX) { // Check GoTo?
distance = axis[Motor].Target - axis[Motor].Position; // Distance to target
if (axis[Motor].ETXMotorStatus & MOVEDECR) // If it is decreasing
distance = TwosComplement(distance);
dbgSerial.printf(" Distance: %6lX, Tgt: %6lX, Pos: %6lX", distance, axis[Motor].Target, axis[Motor].Position);
if (axis[Motor].MotorControl & SlewHBX) { // May need to slew for large changes
axis[Motor].ETXMotorState = ETXSlewMotor; // Slew to M-point
}
else {
axis[Motor].ETXMotorState = ETXCheckSpeed;
axis[Motor].TargetSpeed = axis[Motor].DEGREERATE1; // Set initial speed for 'HIGH SPEED GOTO'
if (distance < axis[Motor].OneDegree) // Scale target speed depending on distance to target
axis[Motor].TargetSpeed = (axis[Motor].TargetSpeed >> 1); // 1/2
if (distance < (axis[Motor].OneDegree >> 2))
axis[Motor].TargetSpeed = (axis[Motor].TargetSpeed >> 1); // 1/4
axis[Motor].ETXSpeed = 0; // Starting from 0
dbgSerial.print(" GoTo-STEP");
}
if (distance < OffsetMax) { // Check for really small moves (< 16 steps)
axis[Motor].ETXMotorState = ETXMotorEnd; // Use Adjust offset
dbgSerial.print(" GoTo-OFFSET");
}
if (distance > (axis[Motor].OneDegree << 3)) { // Always slew for > 8 degrees
axis[Motor].ETXMotorState = ETXSlewMotor;
dbgSerial.print(" GoTo-SLEW");
}
}
}
break;
case ETXSlewMotor:
//dbgSerial.println(""); dbgSerial.print("ETXSlewMotor Motor: "); dbgSerial.print(axis_name[Motor]); dbgSerial.print(" SLEW Cmd: ");
/* Andrew Johansen Melbourne Australia
All gotos are done in 2 phases
In the first phase, it calcs distance to "target minus a set time"
( ie it aims for a spot "behind" the target )
It then starts slewing, at Max speed first if longer than a set distance.
It checks distance intermittently and recalcs speed based on distance to go.
When within a radius of about 40 arcsec it stops.
It then recalcs for a position a few clock seconds ahead of target
and does a second slew to finalise.
It also sets a timer when starting the second slew.
Once within about 10 arcsec of target, it stops until the timer trips
then restarts tracking. ie no hunting around etc at the end,
it just assumes it is correct.
JMA
In the ETX-arduino code, I do similar to what you have said previously.
1. send a high speed command to get somewhere close (originally the breakpoint location)
2. change to a low speed command to get closer (the maximum value of the offset command)
3. read the current position from the ETX and issue an offset command to get to the target
0x06/0x07 Move at fastest rate (reverse/forward)
0x01 Start moving, set speed and direction to the value specified in command
0x00 While moving, adjust speed (but not direction) to the value specified in command
0x02 Adjust position, while moving
0x05 Stop moving
0x08 Return change in position
When, moving the speed set by subsequent 0x00 commands, appears to be ~0.8 of the previous speed.
So the motor asymptotes down to a very low speed. And, at the end the 0x02 command adjusts the final position.
*/
digitalWrite(ETXLED, HIGH); // Turn on the ETX telescope LED
HBXSendCommand(Stop, Motor); // Stop the motor
if (axis[Motor].ETXMotorStatus & MOVEDECR) // -ve i.e. -CCW
axis[Motor].Command = SlewReverse;
else
axis[Motor].Command = SlewForward;
HBXSendCommand(axis[Motor].Command, Motor); // SLEW
axis[Motor].EQGMotorStatus |= MOVEAXIS; // Tell EQx
axis[Motor].ETXSpeed = axis[Motor].DEGREERATE1; // Set ETXSpeed as approx (SLEW=DEGREERATE1) for later speed checks
axis[Motor].ETXMotorState = ETXCheckPosition; // Preset position check
if (axis[Motor].MotorControl & GoToHBX) { // Check if slew was caused by a high speed long distance GoTo
distance = axis[Motor].Target - axis[Motor].Position; // Check Distance to target
if (axis[Motor].ETXMotorStatus & MOVEDECR) // If it is decreasing
distance = TwosComplement(distance);
if (distance < axis[Motor].OneDegree) { // Change to GoTo if within one degree
while(!(HBXSendCommand(Stop, Motor))); // Stop the motor command
axis[Motor].ETXMotorState = ETXStepMotor;
axis[Motor].MotorControl |= SpeedHBX; // Use 0x01 command for next speed
axis[Motor].TargetSpeed = axis[Motor].DEGREERATE1; // Set initial speed
axis[Motor].ETXSpeed = axis[Motor].DEGREERATE1;
axis[Motor].SpeedState = 0;
}
}
if (axis[Motor].MotorControl & SlewHBX) { // Slewing to M-point
axis[Motor].ETXSpeed = axis[Motor].DEGREERATE1; // Indicate current speed (approx)
axis[Motor].ETXMotorState = ETXCheckSlowDown; // Slew until SlowDown
}
//*
dbgSerial.println(""); dbgSerial.print("ETXSlewMotor Motor: "); dbgSerial.print(axis_name[Motor]);
dbgSerial.print(" <sts: "); dbgSerial.print(axis[Motor].ETXMotorStatus, HEX); dbgSerial.print("> ");
dbgSerial.print(", Cmd: "); dbgSerial.print(axis[Motor].Command, HEX);
dbgSerial.print(", Pos: "); dbgSerial.print(axis[Motor].Position, HEX);
dbgSerial.print("->Tgt: "); dbgSerial.print(axis[Motor].Target, HEX);
dbgSerial.print(" Chg: "); dbgSerial.print(axis[Motor].Position-axis[Motor].Target, HEX);
dbgSerial.print(", Inc: "); dbgSerial.print(axis[Motor].Increment, HEX);
dbgSerial.print(", ETXSpeed: "); dbgSerial.print(axis[Motor].ETXSpeed, HEX);
dbgSerial.print(" TgtSpeed: "); dbgSerial.print(axis[Motor].TargetSpeed, HEX);
dbgSerial.print(" SpeedState: "); dbgSerial.println(SpeedStateDesc[(axis[Motor].SpeedState)]);
//*/
break;
case ETXStepMotor:
//*
dbgSerial.println(""); dbgSerial.print("ETXStepMotor Motor: "); dbgSerial.print(axis_name[Motor]);
dbgSerial.print(" <sts: "); dbgSerial.print(axis[Motor].ETXMotorStatus, HEX); dbgSerial.print("> ");
dbgSerial.print(", Cmd: "); dbgSerial.print(axis[Motor].Command, HEX);
dbgSerial.print(", Pos: "); dbgSerial.print(axis[Motor].Position, HEX);
dbgSerial.print("->Tgt: "); dbgSerial.print(axis[Motor].Target, HEX);
dbgSerial.print(" Chg: "); dbgSerial.print(axis[Motor].Position-axis[Motor].Target, HEX);
dbgSerial.print(", Inc: "); dbgSerial.print(axis[Motor].Increment, HEX);
dbgSerial.print(" Speed: "); dbgSerial.print(axis[Motor].ETXSpeed, HEX);
dbgSerial.print(" TargetSpeed: "); dbgSerial.print(axis[Motor].TargetSpeed, HEX);
dbgSerial.print(" SpeedState: "); dbgSerial.println(SpeedStateDesc[(axis[Motor].SpeedState)]);
//*/
digitalWrite(ETXLED, HIGH); // Turn on the LED
if (axis[Motor].MotorControl & SpeedHBX) { // Stepping, High or Low speed
axis[Motor].Command = SpeedStart; // Use SpeedStart to start motion
axis[Motor].MotorControl &= ~SpeedHBX; // Clear flag
}
else
axis[Motor].Command = SpeedChnge; // Use SpeedChnge once started
// Set the speed, and direction
// ----------------------------
P1 = axis[Motor].TargetSpeed;
if (axis[Motor].ETXMotorStatus & MOVEDECR) // 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;
if (HBXSendCommand(axis[Motor].Command, Motor)) { // Send Command, check OK?
HBXSend3Bytes(Motor); // Send the speed
axis[Motor].EQGMotorStatus |= MOVEAXIS; // Tell EQx
axis[Motor].ETXSpeed = axis[Motor].TargetSpeed;
}
else break;
axis[Motor].ETXMotorState = ETXCheckSpeed; // Preset Checkspeed, & if needed, make sure we are up to target speed
if (axis[Motor].MotorControl & GoToHBX) { // If it is a GoTo and up to speed, check position
if (axis[Motor].ETXSpeed == axis[Motor].TargetSpeed)
axis[Motor].ETXMotorState = ETXCheckPosition;
}
else if (axis[Motor].ETXSpeed == 0) { // Stop issued
axis[Motor].ETXMotorState = ETXStopMotor;
}
else if ((axis[Motor].MotorControl & SlewHBX) &&
!(axis[Motor].MotorControl & GoToHBX)) { // SLEW without GoTo = speed change
axis[Motor].ETXMotorState = ETXIdle;
}
else if (axis[Motor].ETXSpeed == axis[Motor].TargetSpeed) { // Else slewing at speed
axis[Motor].ETXMotorState = ETXCheckPosition;
}
break;
case ETXCheckSlowDown:
// Check if Slowdown reached
// Calculate absolute distance to slowdown
// ---------------------------------------
///*
dbgSerial.println(""); dbgSerial.print("ETXCheckSlowDown Motor: "); dbgSerial.print(axis_name[Motor]);
dbgSerial.print(" <sts: "); dbgSerial.print(axis[Motor].ETXMotorStatus, HEX); dbgSerial.print("> ");
dbgSerial.print(", Cmd: "); dbgSerial.print(axis[Motor].Command, HEX);
dbgSerial.print(", Pos: "); dbgSerial.print(axis[Motor].Position, HEX);
dbgSerial.print(" Tgt: "); dbgSerial.print(axis[Motor].Target, HEX);
dbgSerial.print(" Chg: "); dbgSerial.print(axis[Motor].Position-axis[Motor].Target, HEX);
dbgSerial.print(", SD: "); dbgSerial.print(axis[Motor].SlowDown, HEX);
dbgSerial.print(", ETXSpeed: "); dbgSerial.print(axis[Motor].ETXSpeed, HEX);
dbgSerial.print(" TgtSpeed: "); dbgSerial.print(axis[Motor].TargetSpeed, HEX);
//*/
// distance = axis[Motor].SlowDown - axis[Motor].Position;
distance = (axis[Motor].Target - 0x1000) - axis[Motor].Position; // Distance to target
if (axis[Motor].Target < axis[Motor].Position) // If it is decreasing
distance = TwosComplement(distance);
//dbgSerial.print(" distance: ");
//dbgSerial.print(distance, HEX);
if (distance <= 0) {
while(!(HBXSendCommand(Stop, Motor))); // Stop the motor
// HBXSendCommand(Stop, Motor); // Stop the motor
axis[Motor].TargetSpeed = (axis[Motor].SIDEREALRATE << 7); // target is 128xSidereal
axis[Motor].ETXMotorState = ETXCheckSpeed;
axis[Motor].MotorControl &= ~SlewHBX; // Clear slew bit (if it was set)
axis[Motor].MotorControl |= SpeedHBX; // Use 0x01 command for first slow-down
}
break;
case ETXCheckSpeed:
/*
ETXSlew1 1 // 1 x sidereal (0.25 arc-min/sec or 0.0042°/sec)
ETXSlew2 2 // 2 x sidereal (0.50 arc-min/sec or 0.0084°/sec)
ETXSlew3 8 // 8 x sidereal ( 2 arc-min/sec or 0.0334°/sec)
ETXSlew4 16 // 16 x sidereal ( 4 arc-min/sec or 0.0669°/sec)
ETXSlew5 64 // 64 x sidereal ( 16 arc-min/sec or 0.2674°/sec)
ETXSlew6 120 // 30 arc-min/sec or 0.5°/sec
ETXSlew7 240 // 60 arc-min/sec or 1.0°/sec
ETXSlew8 600 // 150 arc-min/sec or 2.5°/sec
ETXSlew9 1080 // 270 arc-min/sec or 4.5°/sec
*/
///*
dbgSerial.println(""); dbgSerial.print("ETXCheckSpeed Motor: "); dbgSerial.print(axis_name[Motor]);
dbgSerial.print(" <sts: "); dbgSerial.print(axis[Motor].ETXMotorStatus, HEX); dbgSerial.print("> ");
dbgSerial.print(", Pos: "); dbgSerial.print(axis[Motor].Position, HEX);
dbgSerial.print(" Tgt: "); dbgSerial.print(axis[Motor].Target, HEX);
dbgSerial.print(" Chg: "); dbgSerial.print(axis[Motor].Position-axis[Motor].Target, HEX);
dbgSerial.print(", Inc: "); dbgSerial.print(axis[Motor].Increment, HEX);
dbgSerial.print(", ETXSpeed: "); dbgSerial.print(axis[Motor].ETXSpeed, HEX);
dbgSerial.print(" TgtSpeed: "); dbgSerial.print(axis[Motor].TargetSpeed, HEX);
//*/
// Speeding Up
// ===========
axis[Motor].ETXMotorState = ETXStepMotor; // Preset set speed as next action
// Ramp up to speed
if ((axis[Motor].TargetSpeed != 0) && (axis[Motor].TargetSpeed > axis[Motor].ETXSpeed)) {
if ((axis[Motor].TargetSpeed - axis[Motor].ETXSpeed) > (axis[Motor].SIDEREALRATE << 6)) { // 64x sidereal
axis[Motor].ETXSpeed += ((axis[Motor].TargetSpeed - axis[Motor].ETXSpeed) >> 1); // Ramp up approx .5 difference
axis[Motor].MotorControl &= ~SpeedHBX; // Use 0x00 command (changes speed, but not direction)
}
else {
axis[Motor].ETXSpeed = axis[Motor].TargetSpeed;
while(!(HBXSendCommand(Stop, Motor))); // Stop the motor command
axis[Motor].MotorControl |= SpeedHBX; // Use 0x01 command (changes speed and direction)
}
}
// Ramp down to speed
else if ((axis[Motor].TargetSpeed != 0) && (axis[Motor].ETXSpeed > axis[Motor].TargetSpeed)) {
axis[Motor].ETXSpeed -= ((axis[Motor].ETXSpeed - axis[Motor].TargetSpeed) >> 2); // Approx .75
if ((axis[Motor].ETXSpeed - axis[Motor].TargetSpeed) <= (axis[Motor].SIDEREALRATE << 7)) {
axis[Motor].ETXSpeed = axis[Motor].TargetSpeed; // Close enough at 128x sidereal, so set the speed
// while(!(HBXSendCommand(Stop, Motor))); // Stop the motor command
axis[Motor].MotorControl &= ~SpeedHBX; // Use 0x00 command (changes speed, but not direction)
}
}
// Ramp down to stop
else if ((axis[Motor].TargetSpeed == 0) && (axis[Motor].ETXSpeed != 0)) {
if (axis[Motor].ETXMotorStatus & MOVESLEW) {
axis[Motor].ETXMotorState = ETXStopMotor;
}
else if (axis[Motor].ETXSpeed >= (axis[Motor].SIDEREALRATE << 7)) { // Ramp down to 128 sidereal
axis[Motor].ETXSpeed -= (axis[Motor].ETXSpeed >> 2); // Approximately .75
while(!(HBXSendCommand(Stop, Motor))); // Stop the motor command
axis[Motor].MotorControl |= SpeedHBX; // Use 0x01 command (changes speed and direction)
}
else
axis[Motor].ETXMotorState = ETXStopMotor; // OK, Stop the motor
}
// Already stopped
else if ((axis[Motor].TargetSpeed == 0) && (axis[Motor].ETXSpeed == 0)) {
axis[Motor].ETXMotorStatus |= MOVESLEW; // ETX Set slewing mode
axis[Motor].ETXMotorStatus &= ~MOVEHIGH; // and speed
axis[Motor].ETXMotorStatus &= ~MOVEAXIS; // Clear the motor moving flag
axis[Motor].MotorControl &= ~GoToHBX; // Clear the GoTo flag
axis[Motor].MotorControl &= ~SlewHBX; // and the slew flag
axis[Motor].EQGMotorStatus |= MOVESLEW; // EQG Set slewing mode
axis[Motor].EQGMotorStatus &= ~MOVEHIGH; // and speed
axis[Motor].EQGMotorStatus &= ~MOVEAXIS; // Clear the motor moving flag
axis[Motor].EQGMotorStatus &= ~MOVEDECR; // Clear the direction flag
axis[Motor].ETXMotorState = ETXCheckStartup;
}
// Switch to position check, when we are at speed - check done in ETXStepMotor
//dbgSerial.print(" oSpeed: "); dbgSerial.print(axis[Motor].ETXSpeed, HEX);
//dbgSerial.print(" oTargetSpeed: "); dbgSerial.print(axis[Motor].TargetSpeed, HEX);
break;
case ETXCheckPosition:
// Check if Target acquired
// ------------------------
// Calculate absolute distance to target
// -------------------------------------
///*
if ( ((Motor == MotorAlt) && ((pAltCount % 10) == 0)) ||
((Motor == MotorAz) && ((pAzCount % 10) == 0)) ) {
dbgSerial.println(""); dbgSerial.print("ETXCheckPosition Motor: "); dbgSerial.print(axis_name[Motor]);
dbgSerial.print(" <sts: "); dbgSerial.print(axis[Motor].ETXMotorStatus, HEX); dbgSerial.print("> ");
dbgSerial.print(", Pos: "); dbgSerial.print(axis[Motor].Position, HEX);
dbgSerial.print("->Tgt: "); dbgSerial.print(axis[Motor].Target, HEX);
dbgSerial.print(" Chg: "); dbgSerial.print(axis[Motor].Position-axis[Motor].Target, HEX);
dbgSerial.print(", Inc: "); dbgSerial.print(axis[Motor].Increment, HEX);
dbgSerial.print(", ETXSpeed: "); dbgSerial.print(axis[Motor].ETXSpeed, HEX);
dbgSerial.print(" TgtSpeed: "); dbgSerial.print(axis[Motor].TargetSpeed, HEX);
}
pAzCount++;
pAltCount++;
//*/
if ( (axis[Motor].MotorControl & SlewHBX) &&
!(axis[Motor].MotorControl & GoToHBX) ) // SLEW without GoTo = speed change
break;
distance = axis[Motor].Target - axis[Motor].Position; // Distance to target
if (axis[Motor].ETXMotorStatus & MOVEDECR) // If it is decreasing
distance = TwosComplement(distance);
if (distance == 0)
axis[Motor].ETXMotorState = ETXMotorEnd;
else if (distance > 0) {
// Start to slow motor if getting near target
// ------------------------------------------
if (distance <= OffsetMax) {
axis[Motor].ETXMotorState = ETXMotorEnd; // Stop motor, set offset
axis[Motor].SpeedState = 0;
}
else if (axis[Motor].SpeedState == 3) {
axis[Motor].TargetSpeed = (axis[Motor].SIDEREALRATE) << 2;
axis[Motor].MotorControl &= ~SpeedHBX; // Use 0x00 command
axis[Motor].ETXMotorState = ETXStepMotor; // Change speed
axis[Motor].SpeedState += 1;
}
else if ((distance <= 0x200) && (axis[Motor].SpeedState == 2)) {
axis[Motor].TargetSpeed = axis[Motor].ETXSpeed >> 2; // 1/16
axis[Motor].MotorControl &= ~SpeedHBX; // Use 0x00 command
axis[Motor].ETXMotorState = ETXStepMotor; // Change speed
axis[Motor].SpeedState += 1;
}
else if ((distance <= 0x400) && (axis[Motor].SpeedState == 1)) {
axis[Motor].TargetSpeed = axis[Motor].ETXSpeed >> 1; // 1/4
axis[Motor].MotorControl &= ~SpeedHBX; // Use 0x00 command
axis[Motor].ETXMotorState = ETXStepMotor; // Change speed
axis[Motor].SpeedState += 1;
}
else if ((distance <= 0x800) && (axis[Motor].SpeedState == 0)) {
axis[Motor].TargetSpeed = axis[Motor].ETXSpeed >> 1; // 1/2
while(!(HBXSendCommand(Stop, Motor))); // Stop the motor command
axis[Motor].MotorControl |= SpeedHBX; // Use 0x01 command
axis[Motor].ETXMotorState = ETXStepMotor; // Change speed
axis[Motor].SpeedState += 1;
}
}
else {
if ((TwosComplement(distance)) > OffsetMax) { // Not sure how good offset is!
// Motor has over-shot the target
// ------------------------------
if (axis[Motor].ETXMotorStatus & MOVEDECR) // ETX -> change direction
axis[Motor].ETXMotorStatus &= ~MOVEDECR;
else
axis[Motor].ETXMotorStatus |= MOVEDECR;
while(!(HBXSendCommand(Stop, Motor))); // Stop the motor command
axis[Motor].MotorControl |= SpeedHBX; // Use 0x01 command
axis[Motor].TargetSpeed = (axis[Motor].SIDEREALRATE) << 2;
axis[Motor].ETXMotorState = ETXStepMotor; // Change ETX speed
}
else{
axis[Motor].ETXMotorState = ETXMotorEnd; // Stop motor, set offset
axis[Motor].SpeedState = 0;
}
}
break;
case ETXStopMotor:
while(!(HBXSendCommand(Stop, Motor))); // Stop the motor
axis[Motor].ETXMotorStatus |= MOVESLEW; // ETX Set slewing mode
axis[Motor].ETXMotorStatus &= ~MOVEHIGH; // and speed
axis[Motor].ETXMotorStatus &= ~MOVEAXIS; // Clear the motor moving flag
axis[Motor].MotorControl &= ~GoToHBX; // Clear the GoTo flag
axis[Motor].MotorControl &= ~SlewHBX; // and the slew flag
axis[Motor].EQGMotorStatus |= MOVESLEW; // EQG Set slewing mode
axis[Motor].EQGMotorStatus &= ~MOVEHIGH; // and speed
axis[Motor].EQGMotorStatus &= ~MOVEAXIS; // Clear the motor moving flag
axis[Motor].EQGMotorStatus &= ~MOVEDECR; // Clear the direction flag
axis[Motor].TargetSpeed = 0;
axis[Motor].ETXSpeed = 0;
axis[Motor].ETXMotorState = ETXCheckStartup;
//jma CheckAltFlipState();
dbgSerial.println(""); dbgSerial.print("ETXStopMotor Motor: "); dbgSerial.print(axis_name[Motor]);
dbgSerial.print(" <sts: "); dbgSerial.print(axis[Motor].ETXMotorStatus, HEX); dbgSerial.print("> ");
dbgSerial.print(", Pos: "); dbgSerial.print(axis[Motor].Position, HEX);
dbgSerial.print("->Tgt: "); dbgSerial.print(axis[Motor].Target, HEX);
dbgSerial.print(" Chg: "); dbgSerial.print(axis[Motor].Position-axis[Motor].Target, HEX);
dbgSerial.print(", Inc: "); dbgSerial.print(axis[Motor].Increment, HEX);
dbgSerial.print(", ETXSpeed: "); dbgSerial.print(axis[Motor].ETXSpeed, HEX);
dbgSerial.print(" TgtSpeed: "); dbgSerial.print(axis[Motor].TargetSpeed, HEX);
break;
case ETXMotorEnd:
///*
dbgSerial.println(""); dbgSerial.print("ETXMotorEnd Motor: "); dbgSerial.print(axis_name[Motor]);
dbgSerial.print(" <sts: "); dbgSerial.print(axis[Motor].ETXMotorStatus, HEX); dbgSerial.print("> ");
dbgSerial.print(", Pos: "); dbgSerial.print(axis[Motor].Position, HEX);
dbgSerial.print("->Tgt: "); dbgSerial.print(axis[Motor].Target, HEX);
dbgSerial.print(" Chg: "); dbgSerial.print(axis[Motor].Position-axis[Motor].Target, HEX);
dbgSerial.print(", Inc: "); dbgSerial.print(axis[Motor].Increment, HEX);
//*/
digitalWrite(ETXLED, LOW); // Turn off the ETX LED
distance = axis[Motor].Target - axis[Motor].Position; // Distance to target
if (axis[Motor].Target < axis[Motor].Position) // If it is decreasing
distance = TwosComplement(distance);
if (distance == 0) {
axis[Motor].ETXMotorState = ETXStopMotor; // Stop the motor
}
else {
axis[Motor].HBXP1 = (distance >> 8) & 0xFF; // Initialize offset bytes
axis[Motor].HBXP2 = distance & 0xFF;
axis[Motor].Command = SetOffset;
if (HBXSendCommand(axis[Motor].Command, Motor)) // Command OK?
HBXSend2Bytes(Motor); // Send the offset
///*
dbgSerial.print(" OFFSET");
dbgSerial.print(" "); dbgSerial.print(axis[Motor].Command, HEX);
dbgSerial.print(" "); dbgSerial.print(axis[Motor].HBXP1, HEX);
dbgSerial.print(" "); dbgSerial.print(axis[Motor].HBXP2, HEX);
dbgSerial.print(" "); dbgSerial.print(axis[Motor].HBXP3, HEX);
//*/
}
axis[Motor].Position = axis[Motor].Target;
axis[Motor].MotorControl &= ~GoToHBX; // Clear the flag
axis[Motor].ETXMotorState = ETXStopMotor;
break;
default:
break;
}
return(true);
}
// Motor functions
bool HBXGetStatus(unsigned char Motor) {
if (!HBXSendCommand(GetStatus, Motor)) {
dbgSerial.println(""); dbgSerial.print("HBXGetStatus Motor: "); dbgSerial.print(axis_name[Motor]); dbgSerial.println(" Cmd Fail");
return(false);
}
HBXGet3Bytes(Motor);
P1 = (axis[Motor].HBXP1 << 8); // Update calculated position
P1 |= axis[Motor].HBXP2; // Convert to 16bits
if (axis[Motor].HBXP1 & 0x80)
P1 |= 0xffff0000; // Sign extend HBXP1 for 2s complement
axis[Motor].Position += P1;
// Wrap the position if above (+12H or +90deg)
if (axis[Motor].Position >= (ETX_CENTRE + (axis[Motor].aVALUE >> 1)))
axis[Motor].Position -= axis[Motor].aVALUE;
// Wrap the position if below (-12H or -90deg)
if (axis[Motor].Position <= (ETX_CENTRE - (axis[Motor].aVALUE >> 1)))
axis[Motor].Position += axis[Motor].aVALUE;
if (StatusCount++ > 51) {
StatusCount = 0;
dbgSerial.print(axis_name[MotorAlt]); dbgSerial.print(": "); dbgSerial.print(axis[MotorAlt].Position, HEX); dbgSerial.print(", ");
dbgSerial.print(axis_name[MotorAz]); dbgSerial.print(": "); dbgSerial.println(axis[MotorAz].Position, HEX);
}
return(true);
}
bool HBXGet2Status(void) {
int i;
do {
i = 0;
if (HBXGetStatus(AzMotor)) i += 1;
TimerDelayuS(HBXBitTime << 2);
if (HBXGetStatus(AltMotor)) i += 1;
TimerDelayuS(HBXBitTime << 2);
} while (i < 2);
return(true);
}
// Calibrate motor LED
// ===================
void CalibrateLEDs(void) {
while (!(HBXSendCommand(Stop, AzMotor))); // Stop both motors
TimerDelaymS(100);
while (!(HBXSendCommand(Stop, AltMotor)));
TimerDelaymS(100);
while (!(HBXGetStatus(AzMotor)))
TimerDelaymS(100);
while (!(HBXGetStatus(AltMotor)))
TimerDelaymS(100);
HBXSendCommand(SleepHBX, AzMotor); // Issue Sleep
TimerDelaymS(100);
HBXSendCommand(CalibrateLED, AzMotor); // Issue Calibrate LED
TimerDelaymS(4000);
HBXSendCommand(SleepHBX, AltMotor);
TimerDelaymS(100);
HBXSendCommand(CalibrateLED, AltMotor);
TimerDelaymS(4000);
HBXSendCommand(GetLEDI, AzMotor); HBXGetByte(AzMotor);
HBXSendCommand(GetLEDI, AltMotor); HBXGetByte(AltMotor);
while (!(HBXGetStatus(AzMotor)))
TimerDelaymS(100);
while (!(HBXGetStatus(AltMotor)))
TimerDelaymS(100);
// Read the calibration
dbgSerial.println(""); dbgSerial.print("Read LEDs - AzMotor: ");
if (HBXSendCommand(GetLEDI, AzMotor))
axis[AzMotor].HBXLEDI = HBXGetByte(AzMotor);
dbgSerial.print(axis[AzMotor].HBXLEDI);
dbgSerial.print(", AltMotor: ");
if (HBXSendCommand(GetLEDI, AltMotor))
axis[AltMotor].HBXLEDI = HBXGetByte(AltMotor);
dbgSerial.print(axis[AltMotor].HBXLEDI);
// Save it to preferences
// Prefences.begin must be set by the caller
preferences.putUChar("AzLEDI", axis[AzMotor].HBXLEDI);
preferences.putUChar("AltLEDI", axis[AltMotor].HBXLEDI);
}
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)
//
bool PAz = false;
bool PAlt = false;
do {
if (!PAz) {
if (HBXSendCommand(GetLEDI, AzMotor))
axis[AzMotor].HBXLEDI = HBXGetByte(AzMotor);
dbgSerial.print("Az HBXLEDI: "); dbgSerial.println(axis[AzMotor].HBXLEDI);
TimerDelaymS(CMNDTIME);
if ((axis[AzMotor].HBXLEDI != 0) && (axis[AzMotor].HBXLEDI != 0xFF))
PAz = true;
}
if (!PAlt) {
if (HBXSendCommand(GetLEDI, AltMotor))
axis[AltMotor].HBXLEDI = HBXGetByte(AltMotor);
TimerDelaymS(CMNDTIME);
dbgSerial.print("Alt HBXLEDI: "); dbgSerial.println(axis[AltMotor].HBXLEDI);
if ((axis[AltMotor].HBXLEDI != 0) && (axis[AltMotor].HBXLEDI != 0xFF))
PAlt = true;
}
// Attempt to reset Motor Controller
if (!PAz) {
HBXMotorReset(AzMotor);
}
if (!PAlt) {
HBXMotorReset(AltMotor);
}
} while ((!PAz) || (!PAlt));
}
void AzInitialise(unsigned char scope) {
// Telescope specific
// telescope steps
axis[AzMotor].Vanes = ratio[scope][AzMotor-1].Vanes;
axis[AzMotor].GbxRatio = ratio[scope][AzMotor-1].GbxRatio;
axis[AzMotor].XferRatio = ratio[scope][AzMotor-1].XferRatio;
axis[AzMotor].WormTeeth = ratio[scope][AzMotor-1].WormTeeth;
// EQMOD
axis[AzMotor].aVALUE = axis[AzMotor].Vanes * (float)4 * axis[AzMotor].GbxRatio * axis[AzMotor].XferRatio * axis[AzMotor].WormTeeth;
axis[AzMotor].MeadeRatio = (float)axis[AzMotor].aVALUE / ArcSecs360; // Count for one arcsec
axis[AzMotor].bVALUE = (MeadeSidereal * axis[AzMotor].MeadeRatio) * ((float)axis[AzMotor].aVALUE / SIDEREALSECS) * SiderealArcSecs;
axis[AzMotor].BASERATE = axis[AzMotor].bVALUE / axis[AzMotor].MeadeRatio;
axis[AzMotor].SIDEREALRATE = MeadeSidereal * axis[AzMotor].MeadeRatio;
axis[AzMotor].SOLARRATE = axis[AzMotor].SIDEREALRATE * SOLARSECS / SIDEREALSECS;
axis[AzMotor].LUNARRATE = axis[AzMotor].SIDEREALRATE * LUNARSECS / SIDEREALSECS;
axis[AzMotor].DEGREERATE1 = axis[AzMotor].SIDEREALRATE * ETXSlew7;
axis[AzMotor].PEC = axis[AzMotor].aVALUE / axis[AzMotor].WormTeeth;
// ETX
axis[AzMotor].HBXP1 = 0x00;
axis[AzMotor].HBXP2 = 0x00;
axis[AzMotor].HBXP3 = 0x00;
axis[AzMotor].HBXP4 = 0x00;
axis[AzMotor].ETXSpeed = 0x00;
axis[AzMotor].SpeedState = 0x00;
axis[AzMotor].TargetSpeed = axis[AzMotor].SIDEREALRATE;
axis[AzMotor].Position = ETX_CENTRE; // ETX RA initially at 0 hours
axis[AzMotor].OneDegree = axis[AzMotor].aVALUE / (float)360; // Distance for one degree
axis[AzMotor].Target = axis[AzMotor].Position;
axis[AzMotor].DirnSpeed = 0x000;
axis[AzMotor].ETXMotorStatus = MOVESLEW;
axis[AzMotor].EQGMotorStatus = MOVESLEW;
axis[AzMotor].ETXMotorState = ETXCheckStartup;
}
void AltInitialise(unsigned char scope) {
// Telescope specific
// telescope steps
axis[AltMotor].Vanes = ratio[scope][AltMotor-1].Vanes;
axis[AltMotor].GbxRatio = ratio[scope][AltMotor-1].GbxRatio;
axis[AltMotor].XferRatio = ratio[scope][AltMotor-1].XferRatio;
axis[AltMotor].WormTeeth = ratio[scope][AltMotor-1].WormTeeth;
// EQMOD
axis[AltMotor].aVALUE = axis[AltMotor].Vanes * (float)4 * axis[AltMotor].GbxRatio * axis[AltMotor].XferRatio * axis[AltMotor].WormTeeth;
axis[AltMotor].MeadeRatio = axis[AltMotor].aVALUE / ArcSecs360; // Count for one arcsec
axis[AltMotor].bVALUE = (MeadeSidereal * axis[AltMotor].MeadeRatio) * (axis[AltMotor].aVALUE / SIDEREALSECS) * SiderealArcSecs;
axis[AltMotor].BASERATE = axis[AltMotor].bVALUE / axis[AltMotor].MeadeRatio;
axis[AltMotor].SIDEREALRATE = MeadeSidereal * axis[AltMotor].MeadeRatio;
axis[AltMotor].SOLARRATE = axis[AltMotor].SIDEREALRATE * SOLARSECS / SIDEREALSECS;
axis[AltMotor].LUNARRATE = axis[AltMotor].SIDEREALRATE * LUNARSECS / SIDEREALSECS;
axis[AltMotor].DEGREERATE1 = axis[AltMotor].SIDEREALRATE * ETXSlew7;
axis[AltMotor].PEC = axis[AltMotor].aVALUE / axis[AltMotor].WormTeeth;
axis[AltMotor].EQGMotorStatus = MOVESLEW;
// ETX
axis[AltMotor].HBXP1 = 0x00;
axis[AltMotor].HBXP2 = 0x00;
axis[AltMotor].HBXP3 = 0x00;
axis[AltMotor].HBXP4 = 0x00;
axis[AltMotor].ETXSpeed = 0x00;
axis[AltMotor].SpeedState = 0x00;
axis[AltMotor].TargetSpeed = axis[AzMotor].SIDEREALRATE;
axis[AltMotor].Position = ETX_CENTRE; // ALTAZ at 0
if (mounttype == 1) axis[AltMotor].Position += (axis[AltMotor].aVALUE >> 2); // GEM Initially at +90 degrees
axis[AltMotor].OneDegree = axis[AltMotor].aVALUE / (float)360; // Distance for one degree
axis[AltMotor].Target = axis[AltMotor].Position;
axis[AltMotor].DirnSpeed = 0x000;
axis[AltMotor].ETXMotorStatus = MOVESLEW;
axis[AltMotor].ETXMotorState = ETXCheckStartup;
}
/*
// Check if ETX mount will hit fork physical limits
// When used as GEM, ETX mounts cannot go past vertical
// This means E in Sthn, W in Nthn, for East side pointing West
// So rotate 180 degrees in Az and flip the Alt destination
*/
void CheckAltFlip(unsigned char Motor) {
long distance;
distance = axis[Motor].Target - axis[Motor].Position; // Distance to target
if ((scopetype < 3) && (Motor == AltMotor)) { // ETXn0 series
long p;
if (distance > 0) {
// GoTo
if (axis[AltMotor].Target >= (ETX_CENTRE + (axis[AltMotor].aVALUE >> 2))) { // E[Sthn] (or W[Nthn])
dbgSerial.println(""); dbgSerial.print("Flip: target "); dbgSerial.print(axis[AltMotor].Target); // Set Target
p = axis[AltMotor].Target - (ETX_CENTRE + (axis[AltMotor].aVALUE >> 2)); // Offset from EQG midpoint
axis[AltMotor].Target -= (p << 1); // Swap to other side
distance = axis[Motor].Target - axis[Motor].Position; // Distance to target
dbgSerial.print(", new "); dbgSerial.print(axis[AltMotor].Target);
// Flip direction
if (axis[AltMotor].ETXMotorStatus & MOVEDECR) axis[AltMotor].ETXMotorStatus &= ~MOVEDECR;
else axis[AltMotor].ETXMotorStatus |= MOVEDECR;
// Set SlowDown
p = axis[AltMotor].SlowDown - (ETX_CENTRE + (axis[AltMotor].aVALUE >> 2));
axis[AltMotor].SlowDown -= (p << 1);
dbgSerial.print(", Slowdown "); dbgSerial.print(p);
if (axis[AltMotor].Flip == NORMAL) {
axis[AltMotor].Flip = FLIP;
dbgSerial.print(", Type "); dbgSerial.print("ALREADY FLIPPED");
}
else {
dbgSerial.print(", Type "); dbgSerial.print("NORMAL");
}
}
else {
if (axis[AltMotor].Flip == FLIPPED) {
axis[AltMotor].Flip = UNFLIP;
dbgSerial.print(", Action "); dbgSerial.print("UNFLIP");
}
else {
dbgSerial.print(", Action "); dbgSerial.print("NOTHING");
}
}
}
else {
// SLEW
if (axis[AltMotor].ETXMotorStatus & MOVEDECR) {
axis[AltMotor].ETXMotorStatus &= ~MOVEDECR;
if (axis[AltMotor].Flip == NORMAL) {
axis[AltMotor].Flip = FLIP;
dbgSerial.print(", Action "); dbgSerial.print("FLIP");
}
else {
dbgSerial.print(", Action "); dbgSerial.print("NOTHING");
}
}
if (axis[AltMotor].Flip == FLIPPED) {
axis[AltMotor].Flip = UNFLIP;
dbgSerial.print(", Action "); dbgSerial.print("FLIP");
}
else {
dbgSerial.print(", Action "); dbgSerial.print("NOTHING");
}
}
}
}
void CheckAltFlipState(void) {
if ((axis[AltMotor].Flip == FLIPPING) || (axis[AltMotor].Flip == UNFLIPPING)) {
dbgSerial.println(""); dbgSerial.print("Stop Motor: ");
if (axis[AltMotor].Flip == FLIPPING) {
axis[AltMotor].Flip = FLIPPED; dbgSerial.print("FLIPPING");
}
else {
axis[AltMotor].Flip = NORMAL; dbgSerial.print("NORMAl");
}
}
}
void CheckAltFlipReqd(void) {
if (axis[AltMotor].ETXMotorState == ETXIdle) {
if ((axis[AltMotor].Flip == FLIP) || (axis[AltMotor].Flip == UNFLIP)) {
if (axis[AltMotor].Flip == FLIP) axis[AltMotor].Flip = FLIPPING;
else axis[AltMotor].Flip = UNFLIPPING;
// Rotate 180 degrees in RA
if (axis[AzMotor].Target > ETX_CENTRE) axis[AzMotor].Target -= (axis[AzMotor].aVALUE >> 1);
else axis[AzMotor].Target += (axis[AzMotor].aVALUE >> 1);
axis[AzMotor].ETXMotorStatus |= (MOVEAXIS || MOVEHIGH);
axis[AzMotor].ETXMotorStatus &= ~MOVESLEW;
axis[AzMotor].MotorControl |= GoToHBX;
CheckETXState(AzMotor);
}
}
}
void PrintHbxValues(unsigned char Motor) {
if (Motor == AzMotor)
dbgSerial.println("AzMotor");
else
dbgSerial.println("AltMotor");
dbgSerial.print("Vanes "); dbgSerial.print(axis[Motor].Vanes);
dbgSerial.print(", GbxRatio "); dbgSerial.print(axis[Motor].GbxRatio,4);
dbgSerial.print(", XferRatio "); dbgSerial.print(axis[Motor].XferRatio,4);
dbgSerial.print(", WormTeeth "); dbgSerial.println(axis[Motor].WormTeeth);
dbgSerial.print("MeadeRatio "); dbgSerial.print(axis[Motor].MeadeRatio,6);
dbgSerial.print(", MeadeSidereal "); dbgSerial.println(MeadeSidereal,4);
dbgSerial.print("aVALUE 0x"); dbgSerial.print(axis[Motor].aVALUE, HEX);
dbgSerial.print(", bVALUE 0x"); dbgSerial.print(axis[Motor].bVALUE, HEX);
dbgSerial.print(", PEC 0x"); dbgSerial.println(axis[Motor].PEC, HEX);
dbgSerial.print("BASERATE 0x"); dbgSerial.print(axis[Motor].BASERATE, HEX);
dbgSerial.print(", SIDEREALRATE 0x"); dbgSerial.print(axis[Motor].SIDEREALRATE, HEX);
dbgSerial.print(", SOLARRATE 0x"); dbgSerial.print(axis[Motor].SOLARRATE, HEX);
dbgSerial.print(", LUNARRATE 0x"); dbgSerial.print(axis[Motor].LUNARRATE, HEX);
dbgSerial.print(", DEGREERATE1 0x"); dbgSerial.println(axis[Motor].DEGREERATE1, HEX);
dbgSerial.print("One DEGREE 0x"); dbgSerial.println(axis[Motor].OneDegree, HEX);
dbgSerial.println("");
}
void PrintRatioValues(unsigned char scope) {
int j;
for (j = 0; j < 2; j++) {
if (j == 0)
dbgSerial.print("AzMotor: ");
else
dbgSerial.print("AltMotor: ");
dbgSerial.print("Vanes "); dbgSerial.print(ratio[scope][j].Vanes);
dbgSerial.print(", GbxRatio "); dbgSerial.print(ratio[scope][j].GbxRatio,4);
dbgSerial.print(", XferRatio "); dbgSerial.print(ratio[scope][j].XferRatio,4);
dbgSerial.print(", WormTeeth "); dbgSerial.print(ratio[scope][j].WormTeeth);
float r = (ratio[scope][j].Vanes * (float) 4 * ratio[scope][j].GbxRatio * ratio[scope][j].XferRatio * ratio[scope][j].WormTeeth) / (float) 1296000;
dbgSerial.print(", MeadeRatio "); dbgSerial.println(r, 6);
}
}
void HBXPrintStatus(unsigned char Motor) {
axis[Motor].HBXCount = 0;
if ((axis[Motor].Command != GetStatus) || (axis[Motor].HBXP1 | axis[Motor].HBXP2 | axis[Motor].HBXP3 | axis[Motor].HBXP4) || axis[Motor].PrintStatus0) {
/* dbgSerial.println("");
dbgSerial.print("Motor: ");
dbgSerial.print(axis_name[Motor]);
dbgSerial.print(", Cmnd: ");
dbgSerial.print(axis[Motor].Command, HEX);
dbgSerial.print(" - ");
*/
switch (axis[Motor].Command) {
case SpeedChnge:
// dbgSerial.print("SpeedChnge ");
axis[Motor].HBXCount = 3;
break;
case SpeedStart:
// dbgSerial.print("SpeedStart ");
axis[Motor].HBXCount = 3;
break;
case SetOffset:
// dbgSerial.print("SetOffset ");
axis[Motor].HBXCount = 4;
break;
case SetLEDI:
// dbgSerial.print("SetLEDI ");
axis[Motor].HBXCount = 1;
break;
case CalibrateLED:
// dbgSerial.print("CalibrateLED ");
break;
case Stop:
// dbgSerial.print("Stop ");
break;
case SlewReverse:
// dbgSerial.print("SlewReverse ");
break;
case SlewForward:
// dbgSerial.print("SlewForward ");
break;
case GetStatus:
// dbgSerial.print("GetStatus ");
axis[Motor].HBXCount = 4;
break;
case GetLEDI:
// dbgSerial.print("GetLEDI ");
axis[Motor].HBXCount = 1;
break;
case GetMotorType:
// dbgSerial.print("GetMotorType ");
axis[Motor].HBXP1 = axis[Motor].MotorType;
axis[Motor].HBXCount = 1;
break;
case SleepHBX:
// dbgSerial.print("SleepHBX ");
break;
default:
dbgSerial.print("UNKNOWN ");
break;
}
/*
if (axis[Motor].HBXCount != 0) {
dbgSerial.print(", Data: ");
dbgSerial.print(axis[Motor].HBXP1, HEX);
if (axis[Motor].HBXCount >= 2) dbgSerial.print(", ");
axis[Motor].HBXCount -= 1;
}
if (axis[Motor].HBXCount != 0) {
dbgSerial.print(axis[Motor].HBXP2, HEX);
if (axis[Motor].HBXCount >= 2) dbgSerial.print(", ");
axis[Motor].HBXCount -= 1;
}
if (axis[Motor].HBXCount != 0) {
dbgSerial.print(axis[Motor].HBXP3, HEX);
if (axis[Motor].HBXCount >= 2) dbgSerial.print(", ");
axis[Motor].HBXCount -= 1;
}
if (axis[Motor].HBXCount > 0) {
dbgSerial.print(axis[Motor].HBXP4, HEX);
axis[Motor].HBXCount -= 1;
}
dbgSerial.println("");
*/
}
}
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik