######################################### telescope_control.py ###################################
#  
from time import sleep, time
from A_to_E_converter import *
import thread
import sys
mypath=sys.path[0]
if mypath.find('my_python') == -1:       #If true, then "my_python" could not be found
  print "Sorry.  You need to be in my_python for Halverson's program to work."
  print "(That''s because I am assuming that the tgraphlix.py and ezu3.py libraries are there.)"
  exit()
sys.path[0]=mypath[0:mypath.find('my_python')]+'my_python'

import pyaudio      #This is to make a beep sound
# Open the stream required, mono mode only...
stream=pyaudio.PyAudio().open(format=pyaudio.paInt8,channels=1,rate=16000,output=True)
#If the line above gives an error, you need to install pyaudio.  It is available on
#Halverson's web site in the Python Program Files area

def beep_1khz():
  #1 kHz tone for 0.1 seconds   I got this from http://code.activestate.com/recipes/578301-platform-independent-1khz-pure-audio-sinewave-gene/
  for n in range(0,100,1): stream.write("\x00\x30\x5a\x76\x7f\x76\x5a\x30\x00\xd0\xa6\x8a\x80\x8a\xa6\xd0")

beep_1khz()   #make a beep sound to say the program is starting
sleep(0.2)
def beep_2khz():
  #1 kHz tone for 0.025 seconds   I got this from http://code.activestate.com/recipes/578301-platform-independent-1khz-pure-audio-sinewave-gene/
  for n in range(0,50,1): stream.write("\x00\x5a\x7f\x5a\x00\xa6\x80\xa6")
sleep(0.2)
beep_2khz()   #make a beep sound to say the program is starting
sleep(0.2)
beep_2khz()   #make a beep sound to say the program is starting

print
import u3
from ezu3_two_devices import *

n_of_devices=u3.deviceCount(3)
print "Number of Labjack devices=",n_of_devices
if n_of_devices < 2:
  print "Something is wrong."
  print "There should be TWO Labjack units connected. One for the motors, one for the controller."
  print "Quitting."
  exit()
devdict = u3.openAllU3()
device_number = 1
for i in devdict.items():
  print "Device #",device_number," has serial number ",i[0]
  device_number=device_number+1

print "If the program crashes now, its because you need to put the right serial numbers in the code."
print "The serial numbers must match the ones just printed out."

motors=devdict['320066905']    #Telescope motor controller     #At Stern MASS
controller=devdict['320066924']    #Boris's hand controller    #At Stern MASS
#motors=devdict['320063858']    #Telescope motor controller      #At Halverson's house
#controller=devdict['320066924']    #Boris's hand controller     #At Halverson's house
u3setup(motors,['ain','ain','dout','din','dout','dout','dout','dout'], \
  u3channelsE=['dout','dout','dout','dout','dout','dout','dout','dout'], \
u3channelsC=['din','din','dout','dout'])
sleep(0.25)
u3setup_2(controller,['din','din','ain','ain','dout','dout','dout','dout'])
sleep(0.25)

altitude_at_step0 = 0    # Global
azimuth_at_step0 = 0     # Global
current_altitude_step=0      # Global.  In this code a "step" is actually a 1/32 step  INTEGER
current_azimuth_step=0        # Global.  In this code a "step" is actually a 1/32 step  INTEGER
target_altitude_step=0.0      # Global.  In this code a "step" is actually a 1/32 step
target_azimuth_step=0.0        # Global.  In this code a "step" is actually a 1/32 step
keep_motor_loop_running = True  # Global
earth_rotation_compensation = False  #Global
earth_rotation_angle = 0.0 # Global

# ============================= start of motor_loop =============================================================
def motor_loop():   #This is a thread (aka task) that runs by itself
  global current_altitude_step
  global current_azimuth_step
  global target_altitude_step
  global target_azimuth_step
  global earth_rotation_compensation
  global earth_rotation_angle

  def set_altitude_microstep_bits(distance_needed):
    #print "set_altitude_microstep_bits: distance_needed=",distance_needed
    distance_needed = distance_needed / 2   # This is like making the Gain = 0.5
    #if distance_needed >= 32:
    #  motor_bits = 0     # Fastest, motor makes full steps
    #  step_multiple=32
    #elif distance_needed >= 16:
    #  motor_bits = 1
    #  step_multiple=16
    if (distance_needed >= 16) & (current_altitude_step % 16 == 0):   #Go to 8/32 steps if needed AND if motor is on a compatible step *
      motor_bits = 1
      step_multiple=16
    elif (distance_needed >= 8) & (current_altitude_step % 8 == 0):   #Go to 8/32 steps if needed AND if motor is on a compatible step *
      motor_bits = 2
      step_multiple=8
    elif (distance_needed >= 4) & (current_altitude_step % 4 == 0):   #Go to 4/32 steps if needed AND if motor is on a compatible step *
      motor_bits = 3
      step_multiple=4
    elif (distance_needed >= 2) & (current_altitude_step % 2 == 0):   #Go to 2/32 steps if needed AND if motor is on a compatible step *
      motor_bits = 4
      step_multiple=2
    else: 
      motor_bits = 5      # Slowest, motor makes 1/32 steps
      step_multiple=1
    #Bits controls the step size.  5=1/32 steps;  4=1/16;  3=1/8;  2=1/4;  1=1/2;  0=whole steps on the Polulu DRV8825
    dout5(motors,motor_bits & 1 != 0)     #microstep select 1 motor 1  altitude motor
    dout6(motors,motor_bits & 2 != 0)     #microstep select 2 motor 1
    dout7(motors,motor_bits & 4 != 0)     #microstep select 3 motor 1
    #print "Altitude step_multiple=",step_multiple
    return step_multiple
    # * Footnote:  If you change to a bigger step size you have to be careful to do it only when the 
    # current step is compatible (i.e. evenly divisible) with that step size.
    # For example, if you want to go to a step size of 4, you can only do it when currently on step 0, 4, 8, 12...
    # See drv8825_controller.pdf page 14, Table 2

  def set_azimuth_microstep_bits(distance_needed):
    #print "set_azimuth_microstep_bits: distance_needed=",distance_needed
    distance_needed = distance_needed / 2    # This is like making the Gain = 0.5
    #if distance_needed >= 32:
    #  motor_bits = 0     # Fastest, motor makes full steps
    #  step_multiple=32
    if (distance_needed >= 16) & (current_azimuth_step % 16 == 0):   #Go to 16/32 steps if needed AND if motor is on a compatible step *
      motor_bits = 1
      step_multiple=16
    elif (distance_needed >= 8) & (current_azimuth_step % 8 == 0):   #Go to 8/32 steps if needed AND if motor is on a compatible step *
      motor_bits = 2
      step_multiple=8
    elif (distance_needed >= 4) & (current_azimuth_step % 4 == 0):   #Go to 4/32 steps if needed AND if motor is on a compatible step *
      motor_bits = 3
      step_multiple=4
    elif (distance_needed >= 2) & (current_azimuth_step % 2 == 0):   #Go to 2/32 steps if needed AND if motor is on a compatible step*
      motor_bits = 4
      step_multiple=2
    else: 
      motor_bits = 5      # Slowest, motor makes 1/32 steps
      step_multiple=1
    #Bits controls the step size.  5=1/32 steps;  4=1/16;  3=1/8;  2=1/4;  1=1/2;  0=whole steps on the Polulu DRV8825
    doutE5(motors,motor_bits & 1 != 0)     #microstep select 1 motor 2  Azimuth motor
    doutE6(motors,motor_bits & 2 != 0)     #microstep select 2 motor 2
    doutE7(motors,motor_bits & 4 != 0)     #microstep select 3 motor 2
    #print "Azimuth step_multiple=",step_multiple
    return step_multiple  

  print "motor_loop starting"
  altitude_step_correction = 0.0
  azimuth_step_correction = 0.0
  previous_target_altitude_step = 0.0
  previous_target_azimuth_step = 0.0
  altitude_step_multiple = set_altitude_microstep_bits(1)   #Slowest motor speed  
  azimuth_step_multiple = set_azimuth_microstep_bits(1)     #Slowest motor speed
  motor_loop_sleeptime = 0.0002        #Time between motor steps (I made it essentially zero)
  earth_rotation_compensation_previous_loop = False
  #Calculate Earth's rotation rate, relative to the stars
  Stellar_Day = 86164.098903691   #Seconds.  From Wikipedia https://en.wikipedia.org/wiki/Earth%27s_rotation#Stellar_and_sidereal_day
  earth_rotation_rate = 360.0 / Stellar_Day    #This gives us degrees per second  So a star will have its phiE increase at this rate
  while keep_motor_loop_running:
    if earth_rotation_compensation:       #==============  YES  Compensation is running ========
      if earth_rotation_compensation_previous_loop:    #=========== Continue compensation
        if time() > earth_rotation_update_time + 2.0:   #Causes the scope to adjust every 2 seconds
          #print "Time to update earth rotation"
          earth_rotation_update_time = time()
          earth_rotation_angle = (earth_rotation_update_time-earth_rotation_start_time)*earth_rotation_rate
          #print "earth_rotation_angle=",earth_rotation_angle
          #Calculate adjustment to azimuth and altitude
          adjusted_phiE = phiE_at_earth_start + earth_rotation_angle    #Add earth rotation to the target's equatorial (hour) angle
          adjusted_thetaA,adjusted_phiA=E_to_A(thetaE_at_earth_start,adjusted_phiE)
          #print "adjusted_thetaA=",adjusted_thetaA," adjusted_phiA=",adjusted_phiA
          altitude_step_correction = (adjusted_thetaA - altitude_at_earth_start)/altitude_step_size
          azimuth_step_correction = (adjusted_phiA - azimuth_at_earth_start)/azimuth_step_size
          #print "current and previous target alt steps:",target_altitude_step,previous_target_altitude_step
          if (target_altitude_step != previous_target_altitude_step) | (target_azimuth_step != previous_target_azimuth_step):
            #================================== If the targets have been changed (user is moving the scope) then reset the corrections
            print "Telescope moving so resetting earth rotation"
            target_altitude_step += altitude_step_correction
            target_azimuth_step += azimuth_step_correction
            earth_rotation_start_time = time()
            earth_rotation_update_time = earth_rotation_start_time
            earth_rotation_angle = 0.0
            altitude_at_earth_start = target_altitude_step*altitude_step_size + altitude_at_step0  
            azimuth_at_earth_start = target_azimuth_step*azimuth_step_size + azimuth_at_step0 
            thetaE_at_earth_start,phiE_at_earth_start=A_to_E(altitude_at_earth_start,azimuth_at_earth_start)
            altitude_step_correction = 0.0
            azimuth_step_correction = 0.0
          else:
            zzz = 0 #do nothing
          #print "altitude_step_correction=",altitude_step_correction,"  azimuth_step_correction=",azimuth_step_correction
          previous_target_altitude_step = target_altitude_step
          previous_target_azimuth_step = target_azimuth_step
      else:
        print("Earth rotation compensation STARTING")
        earth_rotation_start_time = time()
        earth_rotation_update_time = earth_rotation_start_time
        earth_rotation_angle = 0.0
        altitude_at_earth_start = target_altitude_step*altitude_step_size + altitude_at_step0  
        azimuth_at_earth_start = target_azimuth_step*azimuth_step_size + azimuth_at_step0 
        thetaE_at_earth_start,phiE_at_earth_start=A_to_E(altitude_at_earth_start,azimuth_at_earth_start)
        print "altitude_at_step0=",altitude_at_step0,
        print "altitude_at_earth_start=",altitude_at_earth_start,
        print "azimuth_at_step0=",azimuth_at_step0,
        print " azimuth_at_earth_start=",azimuth_at_earth_start,
        print " thetaE_at_earth_start=",thetaE_at_earth_start," phiE_at_earth_start=",phiE_at_earth_start
        altitude_step_correction = 0.0
        azimuth_step_correction = 0.0
        previous_target_altitude_step = target_altitude_step
        previous_target_azimuth_step = target_azimuth_step
        earth_rotation_compensation_previous_loop = True
    else:
      if earth_rotation_compensation_previous_loop:
        print("Earth rotation compensation STOPPING")
        target_altitude_step += altitude_step_correction  #Make the corrections part of target coordinates
        altitude_step_correction = 0.0
        target_azimuth_step += azimuth_step_correction
        azimuth_step_correction = 0.0
        earth_rotation_compensation_previous_loop = False
      else:
        zzz = 0 #do nothing
    #===================== End of Earth rotation compensation code ======================
    sleep(motor_loop_sleeptime)
    #print "motor_loop: current/target_altitude_step=",current_altitude_step,"/",target_altitude_step," current/target_azimuth_step=",current_azimuth_step,"/",target_azimuth_step
    altitude_direction = (target_altitude_step+altitude_step_correction > current_altitude_step) #True means forward
    azimuth_direction = (target_azimuth_step+azimuth_step_correction > current_azimuth_step)       #True means forward
    target_altitude_stepI = int(target_altitude_step+altitude_step_correction)
    target_azimuth_stepI = int(target_azimuth_step+azimuth_step_correction)
    if target_altitude_stepI != current_altitude_step:   #We want to move
      dout2(motors,altitude_direction)    #Direction bit
      altitude_step_multiple=set_altitude_microstep_bits(abs(target_altitude_stepI - current_altitude_step))
      #print "altitude_step_size=", altitude_step_size
    if target_azimuth_stepI != current_azimuth_step:       #We want to move
      doutE2(motors,azimuth_direction)    #Direction bit
      azimuth_step_multiple=set_azimuth_microstep_bits(abs(target_azimuth_stepI - current_azimuth_step))
    if target_altitude_stepI != current_altitude_step:
      dout4(motors,True)     #step command start
    if target_azimuth_stepI != current_azimuth_step:
      doutE4(motors,True)    #step command start
    if target_altitude_stepI != current_altitude_step:
      dout4(motors,False)    #step command finish
      if altitude_direction:
        current_altitude_step += altitude_step_multiple
      else:
        current_altitude_step -= altitude_step_multiple
    if target_azimuth_stepI != current_azimuth_step:
      doutE4(motors,False)   #step command finish
      if azimuth_direction:
        current_azimuth_step += azimuth_step_multiple
      else:
        current_azimuth_step -= azimuth_step_multiple    
  print "Motor loop exiting..."  
# ============================= end of motor_loop =============================================================

want_to_beep = False  #Global
def beep_loop():
  global want_to_beep
  while True:
    sleep(0.2)
    if want_to_beep:
      beep_2khz()
      want_to_beep = False

# Handcontroller variables
keep_controller_loop_running=True    # Global
# ============================= start of controller_loop =============================================================
def controller_loop():
  global target_altitude_step
  global target_azimuth_step
  global want_to_beep
  print "controller_loop starting."
  dout6(controller,False)             #Going up LED
  dout7(controller,False)             #Going down LED
  dout5(controller,False)                 #Going right LED
  dout4(controller,False)                #Going left LED
  speed_range=0.0                     # This gets modified by the controller switches
  switches_old=-999
  deadband_V=0.2                      # Volts This is the half width of the "deadband" inside which there is no motion
  center_V = 2.44/2.0                 # Volts The analog input range of the U3 is 0 to 2.44 Volts
  activeband_V= center_V-deadband_V   # Width of active bands around the deadband
  altitude_control_active1 = False   # Handcontrol is disabled until unlocked by two step process
  altitude_control_active2 = False   # Step 1: get into the deadband  Step 2: get out of deadband
  azimuth_control_active1 = False     #Handcontrol is disabled until unlocked by two step process
  azimuth_control_active2 = False     # Step 1: get into the deadband  Step 2: get out of deadband
  sleeptime = 0.02    #Loop approximately 50 times per second
  while keep_controller_loop_running:
    sleep(sleeptime)
    #  Device 2: Controller built by Boris Bonillo ======================================
    altitude_V=ain2_2(controller)       #Read analog inputs
    azimuth_V=ain3_2(controller)
    #print "Hand control: altitude Volts=",altitude_V," Azimuth Volts=",azimuth_V
    #----------------- altitude control -------------------------------------------
    if altitude_control_active1 & altitude_control_active2:
      #Yes, the control is unlocked
      if abs(altitude_V - (deadband_V + center_V)) > deadband_V:   #Outside of deadband so we want to move
        want_to_beep = True
        target_altitude_step += (altitude_V-center_V)*speed_range*sleeptime
        if (altitude_V-center_V) > 0:
          dout6(controller,True)    #Going up LED
          dout7(controller,False)   #Going down LED
        else:
          dout6(controller,False)   #Going up LED
          dout7(controller,True)    #Going down LED
      else:
        dout6(controller,False)     #Going up LED
        dout7(controller,False)     #Going down LED         
    else:
      if altitude_control_active1:
        if abs(altitude_V - center_V) > deadband_V:
          altitude_control_active2 = True
          print "Unlocking altitude hand control"
          #1 kHz tone for 0.1 seconds   
          #I got this from http://code.activestate.com/recipes/578301-platform-independent-1khz-pure-audio-sinewave-gene/
          for n in range(0,100,1): stream.write("\x00\x30\x5a\x76\x7f\x76\x5a\x30\x00\xd0\xa6\x8a\x80\x8a\xa6\xd0")
      else:
        if abs(altitude_V - center_V) < deadband_V:   #Must be inside deadband to start
          altitude_control_active1 = True
    #----------------- azimuth control ---------------------------------------------
    if azimuth_control_active1 & azimuth_control_active2:
      #Yes, the control is unlocked
      if abs(azimuth_V - (deadband_V + center_V)) > deadband_V:   #Outside of deadband so we want to move
        want_to_beep = True
        target_azimuth_step += (azimuth_V-center_V)*speed_range*sleeptime
        if (azimuth_V-center_V) > 0:
          dout5(controller,True)    #Going right LED
          dout4(controller,False)   #Going left LED
        else:
          dout5(controller,False)   #Going right LED
          dout4(controller,True)    #Going left LED
      else:
        dout5(controller,False)                #Going right LED
        dout4(controller,False)                #Going left LED
    else:
      if azimuth_control_active1:
        if abs(azimuth_V - center_V) > deadband_V:
          azimuth_control_active2 = True
          print "Unlocking azimuth hand control"
          #1 kHz tone for 0.1 seconds   
          #I got this from http://code.activestate.com/recipes/578301-platform-independent-1khz-pure-audio-sinewave-gene/
          for n in range(0,100,1): stream.write("\x00\x30\x5a\x76\x7f\x76\x5a\x30\x00\xd0\xa6\x8a\x80\x8a\xa6\xd0")
      else:
        if abs(azimuth_V - center_V) < deadband_V:  #Must be inside deadband to start
          azimuth_control_active1 = True
    #----------------- switches --------------------------------------------------
    # Controls the speed range depending on the hand controller switch settings
    switch0=din0_2(controller)
    switch1=din1_2(controller)
    if not switch0:
      switches = 1
    else:
      switches = 0  
    if not switch1:
      switches = switches + 2
    if switches != switches_old:   #Check if the switches have changed, if they have,
    #                               then go ahead and change the motor step sizes.
      switches_old = switches
      if switches == 0:
        speed_range=55.0
      elif switches == 1:
        speed_range=166.0
      elif switches == 2:
        speed_range=500.0
      elif switches== 3:
        speed_range=1500.0
# ============================= end of controller_loop =============================================================

# ============================= start of main program =============================================================
from keystroke_lib import *
print "Main program starting"
thread.start_new_thread(controller_loop,())  #Start the controller_loop
sleep(0.2) #Wait 1/2 sec so threads don't start simultaneously
thread.start_new_thread(motor_loop,())        #Start the motor_loop
sleep(0.2)
thread.start_new_thread(beep_loop,())        #Start the beep_loop
sleep(0.2)
with KeyPoller() as keyPoller:   
# "with" means the object keyPoller of the KeyPoller class is going to be used
# in such a way that the __enter__ code is automatically executed at the start and the
# __exit__ code is executed at the end.
# "as" seems to be needed the give a name to refer to the class that doesn't conflict with itself
# https://stackoverflow.com/questions/1984325/explaining-pythons-enter-and-exit
  def getfloat():     #I wrote this to allow entering numbers in spite of the keyboard
        # being taken over by keyPoller
        keep_looping = True
        number_string = ""
        while keep_looping:
          sleep(0.05)
          c=keyPoller.poll()     #Get a character from the keyboard
          if not c is None:
            if ord(c) == 10:    #The "return" key is 10
              print
              #print "Return Key."
              try:
                x=float(number_string)
              #except:
                #print "Something is wrong with the number string"
              #else:
                #print "x=",x
              finally:
                #print "Going back to main loop"
                keep_looping = False
            else:  
              #print "Get number loop got c=",c," which is ascii code ",ord(c)
              #print c,
              number_string += c
              sys.stdout.write(c)  #Echo the keystrokes
              sys.stdout.flush()   #Make sure the keystrokes are sent to the screen immediately
              #print "number_string=",number_string
        return x   
    
  azimuth_step_size = 0.001578105  #Degrees  This is how much the telescope moves for each 1/32nd step (Need to calibrate)
  altitude_step_size = 0.001357496 #Degrees  This is how much the telescope moves for each 1/32nd step
  c = "?"
# ============================= start of main loop =============================================================
  while c != "q":
    sleep(1.0)
    current_azimuth = current_azimuth_step*azimuth_step_size + azimuth_at_step0
    if current_azimuth < 0.0:
      current_azimuth += 360.0
    current_altitude = current_altitude_step*altitude_step_size + altitude_at_step0
    current_thetaE,current_phiE=A_to_E(current_altitude,current_azimuth)  
    current_phiHA=(current_phiE+180.)/15.  #Hour angle, angle relative to the meridian plane, converted to Hours
                          #See en.wikipedia.org/wiki/Hour_angle
                          #This is what Stellarium uses
    if current_phiHA > 24.0:
      current_phiHA -= 24.0
    print "earth rot=%-7.3f " % earth_rotation_angle,
    print "current/target_alt_step=%7d"%current_altitude_step,
    print "/%-9.1f" % target_altitude_step,
    print " current/target_az_step=%7d"%current_azimuth_step,
    print "/%-9.1f" % target_azimuth_step,
    print "Az=%-7.3f" % current_azimuth,
    print "Alt=%-7.3f" % current_altitude,
    #print " Az= ",current_azimuth,
    #print " Alt=",current_altitude,
    print " PhiEq.=%-7.3f" % current_phiE,
    print "HA=%-7.4fH" % current_phiHA,
    print "Dec=%-7.3f" % current_thetaE
    if not c is None:
      #print "Main loop got c=",c," which is ascii code ",ord(c)
      if c == "?":
        print "? = Give this help.        q=quit"
        print "c=calibrate azimuth and altitude using a known star's Az and Alt"
        print "C=calibrate using hour angle and declination"
        print "z=calibrate Azimuth (compass heading) using a known azimuth"
        print "e=calibrate ELEVATION (Altitude) using a known altitude"
        print "h=calibrate using a star's known Hour Angle"
        print "d=calibrate using a star's known Declination"
        print "t=target a star using Azimuth and Altitude"
        print "T=target a star using using Hour Angle and Declination"
        print "r=turn on/off earth rotation compensation"
        print "s=stop telescope motion (stops the t and T commands)"
        print "S=Stop main loop (for debugging)"
      elif c=="c":
        print
        print "Currently azimuth_at_step0=",azimuth_at_step0
        print "Please enter the known Azimuth or compass heading in degrees:"
        try:
          current_azimuth = getfloat()
          azimuth_at_step0 = current_azimuth - current_azimuth_step*azimuth_step_size
          if azimuth_at_step0 < 0.0:
            azimuth_at_step0 += 360.0
          print "New azimuth_at_step0=",azimuth_at_step0
          print "Currently altitude_at_step0=",altitude_at_step0
          print "Please enter the known Altitude or elevation in degrees:"
          try:
            current_altitude = getfloat()
            altitude_at_step0 = current_altitude - current_altitude_step*altitude_step_size
            if altitude_at_step0 < 0.0:
              altitude_at_step0 += 360.0
            print "New altitude_at_step0=",altitude_at_step0
          except:
            print "Error getting number"  
        except:
          print "Error getting number"  
      elif c=="e":
        print
        print "Currently altitude_at_step0=",altitude_at_step0
        print "Please enter the known Altitude or elevation in degrees:"
        try:
          current_altitude = getfloat()
          altitude_at_step0 = current_altitude - current_altitude_step*altitude_step_size
          if altitude_at_step0 < 0.0:
            altitude_at_step0 += 360.0
          print "New altitude_at_step0=",altitude_at_step0
        except:
          print "Error getting number"  



      elif c=="C":
        print
        print "Please enter the HA of the known star, in hours:"
        try:
          current_phiHA = getfloat()
          current_phiE = (current_phiHA+12.0)*15.0
          if current_phiE >= 360.0:
            current_phiE -= 360.0
          print "Please enter the Dec of the known star, in degrees:"
          try:
            current_thetaE = getfloat()            
            current_altitude,current_azimuth=E_to_A(current_thetaE,current_phiE)  #convert (thetaE,phiE) to (thetaA,phiA)
            azimuth_at_step0 = current_azimuth - current_azimuth_step*azimuth_step_size
            if azimuth_at_step0 < 0.0:
              azimuth_at_step0 += 360.0
            print "New azimuth_at_step0=",azimuth_at_step0
            altitude_at_step0 = current_altitude - current_altitude_step*altitude_step_size
            if altitude_at_step0 < 0.0:
              altitude_at_step0 += 360.0
            print "New altitude_at_step0=",altitude_at_step0
          except:
            print "Error getting number"
        except:
          print "Error getting number"  
      elif c=="z":
        print
        print "Currently azimuth_at_step0=",azimuth_at_step0
        print "Please enter the known Azimuth or compass heading in degrees:"
        try:
          current_azimuth = getfloat()
          azimuth_at_step0 = current_azimuth - current_azimuth_step*azimuth_step_size
          if azimuth_at_step0 < 0.0:
            azimuth_at_step0 += 360.0
          print "New azimuth_at_step0=",azimuth_at_step0
        except:
          print "Error getting number"  
      elif c=="e":
        print
        print "Currently altitude_at_step0=",altitude_at_step0
        print "Please enter the known Altitude or elevation in degrees:"
        try:
          current_altitude = getfloat()
          altitude_at_step0 = current_altitude - current_altitude_step*altitude_step_size
          if altitude_at_step0 < 0.0:
            altitude_at_step0 += 360.0
          print "New altitude_at_step0=",altitude_at_step0
        except:
          print "Error getting number"  
      elif c=="a":
        print
        print "Currently azimuth_at_step0=",azimuth_at_step0
        print "Please enter the known Azimuth or compass heading in degrees:"
        try:
          current_azimuth = getfloat()
          azimuth_at_step0 = current_azimuth - current_azimuth_step*azimuth_step_size
          if azimuth_at_step0 < 0.0:
            azimuth_at_step0 += 360.0
          print "New azimuth_at_step0=",azimuth_at_step0
        except:
          print "Error getting number"  
      elif c=="e":                               #Calibrate using elevation or altitude
        print
        print "Currently altitude_at_step0=",altitude_at_step0
        print "Please enter the known Altitude or elevation in degrees:"
        try:
          current_altitude = getfloat()
          altitude_at_step0 = current_altitude - current_altitude_step*altitude_step_size
          if altitude_at_step0 < 0.0:
            altitude_at_step0 += 360.0
          print "New altitude_at_step0=",altitude_at_step0
        except:
          print "Error getting number"  
      elif c=="h":
        print
        print "Please enter the HA of the known star, in hours:"
        try:
          current_phiHA = getfloat()
          current_phiE = (current_phiHA+12.0)*15.0
          if current_phiE >= 360.0:
            current_phiE -= 360.0
          current_altitude,current_azimuth=E_to_A(current_thetaE,current_phiE)  #convert (thetaE,phiE) to (thetaA,phiA)
          azimuth_at_step0 = current_azimuth - current_azimuth_step*azimuth_step_size
          if azimuth_at_step0 < 0.0:
            azimuth_at_step0 += 360.0
          print "New azimuth_at_step0=",azimuth_at_step0
          altitude_at_step0 = current_altitude - current_altitude_step*altitude_step_size
          if altitude_at_step0 < 0.0:
            altitude_at_step0 += 360.0
          print "New altitude_at_step0=",altitude_at_step0
        except:
           print "Error getting number"  
      elif c=="d":
        print
        print "Please enter the Dec of the known star, in degrees:"
        try:
          current_thetaE = getfloat()            
          current_altitude,current_azimuth=E_to_A(current_thetaE,current_phiE)  #convert (thetaE,phiE) to (thetaA,phiA)
          azimuth_at_step0 = current_azimuth - current_azimuth_step*azimuth_step_size
          if azimuth_at_step0 < 0.0:
            azimuth_at_step0 += 360.0
          print "New azimuth_at_step0=",azimuth_at_step0
          altitude_at_step0 = current_altitude - current_altitude_step*altitude_step_size
          if altitude_at_step0 < 0.0:
            altitude_at_step0 += 360.0
          print "New altitude_at_step0=",altitude_at_step0
        except:
          print "Error getting number"
      elif c=="t":
        print
        print "Please enter the Azimuth of the target star, in degrees:"
        try:
          target_azimuth = getfloat()
          
          print "Please enter the Altitude of the target star, in degrees:"
          try:
            target_altitude = getfloat()
            target_altitude_step=(target_altitude-altitude_at_step0)/altitude_step_size
            target_azimuth_step=(target_azimuth-azimuth_at_step0)/azimuth_step_size
            print "OK:  Moving telescope to step",target_altitude_step,target_azimuth_step
          except:
            print "Error getting number"
        except:
          print "Error getting number"  
      elif c=="T":
        print
        print "Please enter the Hour Angle of the target star, in decimal hours:"
        try:
          target_Hour_Angle = getfloat()
          target_phiE=(target_Hour_Angle+12.0)*15.0
          if target_phiE >= 360.0:
            target_phiE -= 360.0
          print "Please enter the Declination of the target star, in degrees:"
          try:
            target_Declination = getfloat()
            target_altitude,target_azimuth=E_to_A(target_Declination,target_phiE)
            target_altitude_step=(target_altitude-altitude_at_step0)/altitude_step_size
            target_azimuth_step=(target_azimuth-azimuth_at_step0)/azimuth_step_size
            print "OK:  Moving telescope to step",target_altitude_step,target_azimuth_step
          except:
            print "Error getting number"
        except:
          print "Error getting number"  
      elif c == "r":
        earth_rotation_compensation = not earth_rotation_compensation
        if earth_rotation_compensation:
          print "ROTATION COMPENSATION ON"
        else:
          print "ROTATION COMPENSATION OFF"
      elif c == "s":
        print "STOP"
        target_altitude_step=current_altitude_step
        target_azimuth_step=current_azimuth_step
      elif c == "S":
        print "Press any key to continue"
        c = None
        while c is None:
          sleep(0.1)
          c = keyPoller.poll()      #Get a character from the keyboard
      elif c == "q":
        break
    c = keyPoller.poll()      #Get a character from the keyboard  
  print "Main program finished"
  print


