' {$STAMP BS2p}
' {$PBASIC 2.5}
' {$PORT COM1}

#DEFINE DebugMode

' NAME THIS FILE run3.bsp  FIRST LINE MUST BE ~ ' {$STAMP BS2p...

' run0.bsp, run1.bsp, run2.bsp, run3.bsp

' Version 12/09/2010

' Memory location 0-1440 minutes in 24 hours stores azimuth sensor and altitude sensor (when focused every 4 minutes)
' and declination (brads+128) AND 0.

' MUST DO THIS WITH NEW EQUIPMENT -- Memory Erase Start 0 End 2047
' This fills all EEPROM locations with zeros, the notebook will make beeps indicating Write instructions.

' Set clock standard time (not daylight savings time) with solar noon correction (if known).
' Set time minus correction IF solar noon past noon OR REVERSE.
' Each sunny noon will reset clock TO approximately solar noon.

' Memory location 1500-1900 stores stow data -- Type, Time/10, Azimuth_Sensor, Altitude_Sensor
' Type:
' 1 Azimuth sensor failure
' 2 Altitude sensor failure
' 3 Time failure
' 4 Azimuth motor failed to move
' 5 Altitude motor failed to move (check limit switches)
' 6 Eye failure
' 7 Azimuth  eye/sensor/clock/path error.  Check all four.
' 8 Altitude eye/sensor/clock/path error.  Check all four.
' 9 Solar_noon clock/position error.



cport           PIN      0        ' I2C - DS1307 clock
cclock          PIN      1        ' I2C
North_eye       PIN      2        ' RC Time
South_eye       PIN      3        ' RC Time
East_eye        PIN      5        ' RC Time
West_eye        PIN      4        ' RC Time
'water_off       PIN      6
tm              PIN      7        ' Motor Mind C serial comm. SERIN
fm              PIN      8        ' Motor Mind C serial comm. SEROUT
Xin             PIN      9        ' Rotate X Accelerometer MEMSIC 2125
Yin             PIN      10       ' Rotate Y Accelerometer MEMSIC 2125
Zin             PIN      11       ' Tilt   Z Accelerometer MEMSIC 2125 Altitude_sensor
'temp_1          PIN      12
'temp_2          PIN      13
SW1             PIN      14       ' Pulled high
SW2             PIN      15       ' Pulled high

speed_azimuth   VAR      Word      'PWM storage register for motor 1
speed_altitude  VAR      Word      'PWM storage register for motor 2
time            VAR      Word

work            VAR      Word
work1           VAR      Word
work2           VAR      Word
work3           VAR      Word
work4           VAR      Word
work5           VAR      Word

Azimuth_sensor  VAR      Byte
Altitude_sensor VAR      Byte

flags           VAR      Byte
flag_north      VAR      flags.BIT0
flag_south      VAR      flags.BIT1
flag_run        VAR      flags.BIT2
flag_stow       VAR      flags.BIT3

d               VAR      Byte    ' Declination

Dwell           VAR      Nib
Azimuth_Motor   VAR      Nib
Altitude_Motor  VAR      Nib
Smooth          VAR      Nib

read_flag       VAR      Bit
write_flag      VAR      Bit
flag            VAR      Bit
dark            VAR      Bit
stow_write      VAR      Bit
solar_noon      VAR      Bit

sign            VAR      Smooth.BIT0
cloudy          VAR      Smooth.BIT1
Focus           VAR      Smooth.BIT2

Azimuth_Stow          VAR      Bit
Altitude_Stow         VAR      Bit

azimuth_command       VAR      Bit
altitude_command      VAR      Bit
azimuth_go            VAR      Bit
altitude_go           VAR      Bit

azimuth_spin          VAR      Bit
altitude_spin         VAR      Bit
azimuth_on            VAR      Bit
altitude_on           VAR      Bit

xRaw            VAR      work1
yRaw            VAR      work2
zRaw            VAR      work3

p2hi            VAR      speed_azimuth.HIGHBYTE
p2lo            VAR      speed_azimuth.LOWBYTE
p1hi            VAR      speed_altitude.HIGHBYTE
p1lo            VAR      speed_altitude.LOWBYTE

CKSUM           VAR      work.LOWBYTE
key             VAR      work.LOWBYTE

dat1            VAR      work.HIGHBYTE
value           VAR      work.HIGHBYTE
set             VAR      work.HIGHBYTE

sec             VAR      work1.HIGHBYTE
minute          VAR      work1.LOWBYTE
hour            VAR      work2.HIGHBYTE
day             VAR      work2.LOWBYTE
date            VAR      work3.HIGHBYTE
month           VAR      work3.LOWBYTE
year            VAR      work.HIGHBYTE

'lat             CON      34   '47 degrees Tacoma = 34 brads  (binary radians 0-255 in circle)

BAUD            CON      45
rampAz          CON      8
rampAlt         CON      64
startAz         CON      368
startAlt        CON      428
maxAz           CON      832      'max 1023
maxAlt          CON      1023     'max 1023

Scale           CON      $0C0

HiPulse         CON      1
LoPulse         CON      0
on_             CON      1
off             CON      0
up              CON      1
down            CON      0
right           CON      1
left            CON      0
true            CON      1
false           CON      0
yes             CON      1
no              CON      0
go              CON      1
stop_           CON      0


STORE 7

Keys:      'Keyboard interface

#IF DebugMode #THEN DEBUG CLS #ENDIF

IF SW1 OR SW2 THEN RUN 2

#IF DebugMode #THEN

PAUSE 100
DEBUG  CLS,"1 Motor",CR,"2 clock",CR,"3 memory ",CR,"4 exit - Must flip one or both switches first",CR
DEBUGIN DEC1 key
DEBUG CLS
SELECT key
  CASE 1
  GOTO motor
  CASE 2
  GOTO SET_clock
  CASE 3
  GOTO memory
  CASE ELSE
  GOTO Keys
ENDSELECT

motor:

   PAUSE 100
DEBUG  HOME, "1 W   2 E   3 stop   4 N   5 S   6 stop   7 refresh   8 exit"

DEBUGIN DEC1 key
DEBUG CRSRXY,10,3,"Azimuth",CRSRX,20,"Altitude"
SELECT key
  CASE 1
    azimuth_command = right
    azimuth_go = yes
  CASE 2
    azimuth_command = left
    azimuth_go = yes
  CASE 3
    azimuth_go = no
  CASE 4
    altitude_command = up
    altitude_go = yes
  CASE 5
    altitude_command = down
    altitude_go = yes
  CASE 6
    altitude_go = no
  CASE 7
    GOTO E_OK
  CASE ELSE
    GOTO keys
ENDSELECT
IF key > 3 THEN Altitude

Azimuth:

work1 = speed_azimuth

IF azimuth_on = yes AND azimuth_command <> azimuth_spin  THEN  azimuth_go = no  ' stop on this cycle before reverse
IF azimuth_command = right THEN work1 = ((speed_azimuth + rampAz) * azimuth_go) ELSE work1 = (speed_azimuth - rampAz) * azimuth_go
IF work1 = 0 OR ABS(work1) >= startAz THEN Azimuth_start_OK

IF work1.BIT15 = 0 THEN work1 = startAz ELSE work1 = -startAz

Azimuth_start_OK:

IF ABS(work1) <= maxAz THEN Azimuth_OK
IF work1.BIT15 = 0 THEN work1 = maxAz ELSE work1 = -maxAz  'IF work1 < -maxAz THEN work1 = -maxAz

Azimuth_OK:
speed_azimuth = work1
GOTO Set_Motor

Altitude:

work2 = speed_altitude

IF altitude_on = yes AND altitude_command <> altitude_spin  THEN  altitude_go = no  ' stop on this cycle before reverse
IF altitude_command = up THEN work2 = (speed_altitude + rampAlt) * altitude_go ELSE work2 = (speed_altitude - rampAlt) * altitude_go
IF work2 = 0 OR ABS(work2) >= startAlt THEN Altitude_start_OK

IF work2.BIT15 = 0 THEN work2 = startAlt ELSE work2 = -startAlt

Altitude_start_OK:

IF ABS(work2) <= maxAlt THEN Altitude_OK
IF work2.BIT15 = 0 THEN work2 = maxAlt ELSE work2 = -maxAlt  'IF work2 < -maxAlt THEN work2 = -maxAlt

Altitude_OK:
speed_altitude = work2

Set_Motor:

#IF DebugMode #THEN DEBUG CRSRXY,0,5,"Speed",CLREOL,CRSRX,10,SDEC speed_azimuth,CRSRX,20,SDEC speed_altitude #ENDIF

CKSUM = $D0+$01+$04+p1hi+p1lo+p2hi+p2lo
  SEROUT  FM,BAUD,[$D0,$01,$04,P1HI,P1LO,P2HI,P2LO,CKSUM]
  SERIN   TM,BAUD,150,motor_error,[DAT1]
  IF    DAT1 <> $6 THEN GOTO motor_error ELSE GOTO E_OK

motor_error:
#IF DebugMode #THEN DEBUG CRSRXY,2,22," MOTOR ERROR",CR,HEX dat1," ",HEX cksum,7,CR
  PAUSE 5000
#ENDIF

E_OK:

work3 = 0
work4 = 0

FOR work.HIGHBYTE = 1 TO 32
  PULSIN Xin, HiPulse, xRaw
  xRaw = (xRaw */ Scale) / 10
  work3 = work3 + xRaw
  PULSIN Yin, HiPulse, yRaw
  yRaw = (yRaw */ Scale) / 10
  work4 = work4 + yRaw
NEXT
work3 = work3 / 32
work4 = work4 / 32
  IF work3 < 320 OR work3 > 680 OR work4 < 320 OR work4 > 680 THEN Altitude_Stow = yes
xRaw = work3 MIN 373 MAX 627
xRaw = xRaw - 500
yRaw = work4 MIN 373 MAX 627
yRaw = yRaw - 500
Azimuth_sensor =  xRaw ATN yRaw

'Reverse sensor to maintain positive west
'Azimuth_sensor = 255 - Azimuth_sensor

work4 = 0

FOR work.HIGHBYTE = 1 TO 64
  PULSIN Zin, HiPulse, zRaw
  zRaw = (zRaw */ Scale) / 10
  work4 =  work4 + zRaw
NEXT
work4 = work4 / 64
  IF work4 < 320 OR work4 > 680 THEN Azimuth_Stow = yes
work4 = (work4 - 373) MAX 255
  IF work4 < 1 THEN work4 = 1
Altitude_sensor = work4

'Reverse sensor to maintain positive up
Altitude_sensor = 255 - Altitude_sensor
#IF DebugMode #THEN DEBUG CRSRXY, 0,7,"Sensors",CLREOL,CRSRX,10,DEC Azimuth_Sensor,CRSRX,20,DEC Altitude_Sensor #ENDIF
GOTO motor

set_clock:

#IF DebugMode #THEN DEBUG CLS
#ENDIF
I2CIN cport,$d1,0,[sec,minute,hour,day,date,month,year]            ' Read from DS1307 registers
time = (hour.HIGHNIB*10+hour.LOWNIB)*60+(minute.HIGHNIB*10+minute.LOWNIB)
                                                   ' decNum = (bcdNum.NIB1 * 10) + bcdNum.NIB0
                                                   ' bcdNum = (decNum / 10 << 4) + (decNum // 10)
#IF DebugMode #THEN DEBUG CLS,CRSRXY, 0,1,HEX2 month,"/",HEX2 date,"/",HEX2 year,"  day ",HEX2 day,"   ",
HEX2 hour,":",HEX2 minute,":",HEX2 sec,"   Time ",DEC time
#ENDIF
#IF DebugMode #THEN DEBUG CRSRXY, 0,3,"0 sec",CR,"1 min",CR,"2 hr",CR,"3 day",CR,"4 date",CR,"5 mo",CR,"6 yr",CR,"7 reset",CR,
"8 EXIT",CR
#ENDIF

#IF DebugMode #THEN DEBUGIN DEC1 key
#ENDIF
IF key > 7 THEN
#IF DebugMode #THEN DEBUG CLS
#ENDIF
GOTO keys
ENDIF

IF key = 7 THEN GOTO reset

#IF DebugMode #THEN DEBUG CRSRXY,7, key+3,"     Set "
#ENDIF
#IF DebugMode #THEN DEBUGIN HEX2 set
#ENDIF

I2COUT cport,$d0,key,[set]                           ' Write to DS1307 register

GOTO set_clock

reset:

FOR sec = 7 TO 0                                ' DS1307 clock reset and start crystal
I2COUT cport,$d0,sec,[0]                        ' run with backup battery replacement
NEXT

GOTO set_clock

Memory:

#IF DebugMode #THEN
DEBUG "Memory: 0 erase, 1 read, 2 ATN-flag, 5 exit",CR
DEBUGIN DEC1 key
IF key > 2 THEN keys
IF key = 2 THEN jump
DEBUG "Start memory location  "
DEBUGIN DEC work1
DEBUG DEC work1,CR
DEBUG "End "
DEBUGIN DEC work2
DEBUG DEC work2,CR
 work4.LOWBYTE=0: work4.HIGHBYTE=0: work5.LOWBYTE=0:  work5.HIGHBYTE=0
FOR work3 = work1 TO work2 STEP 4

IF key THEN
  READ work3,  work4.LOWBYTE,  work4.HIGHBYTE,  work5.LOWBYTE,  work5.HIGHBYTE
  DEBUG DEC work3, CRSRX, 10, DEC work4.LOWBYTE, CRSRX, 20, DEC work4.HIGHBYTE,
  CRSRX, 30, DEC work5.LOWBYTE,CRSRX, 40, DEC work5.HIGHBYTE,CR
  ELSE
  WRITE work3, work4.LOWBYTE,  work4.HIGHBYTE,  work5.LOWBYTE,  work5.HIGHBYTE
  DEBUG 7
ENDIF
NEXT
jump:
DEBUG "REWRITE ATN_FLAG? 9 "
DEBUGIN DEC1 key
IF key = 9 THEN RUN 4
GOTO memory
#ENDIF
GOTO keys
#ENDIF
GOTO keys