' {$STAMP BS2p} ' {$PBASIC 2.5} ' {$PORT COM1} '#DEFINE DebugMode ' NAME THIS FILE run2.bsp FIRST LINE MUST BE ~ ' {$STAMP BS2p... ' run0.bsp, run1.bsp, run2.bsp, run3.bsp ' Version 6/10/2010 ' Get_Sensors: XY is Memsic rolled on edge with brads output, but mechanical link not exactly a brad circle so scaling result required. ' Z is Memsic tilt with near linear output. 'Get_Time: IF work4 < time OR time = 0 THEN new day, calculate brad declination. ' IF clock NOT advancing THEN stow. ' IF a motor NOT advancing THEN stow. ' work4.BIT2 is every 4 minutes. work4.BIT0 is every 1 minute. ' GOSUB Get_Path calculates sun position every minute, requires clock adjusted to solar noon. ' A routine in Eyes maintains clock accuracy based ON sun position. Solar time. ' Dwell is count down timer for start cloudy tracking following end of sunny tracking. ' Get_Eyes: ' Smaller number is brighter. ' IF too BRIGHT THEN eye failure ~ stow. ' Cloudy. ' IF one side of each eye sees sun THEN NOT cloudy. ' IF dark AND all eyes see brighter than twilight and not evening THEN morning. ' IF any eye sees night THEN dark. ' Set motor commands. ' declination = d + 128 makes declination all positive for memory drift calculations. ' Get_Memory: read_flag is every 4 minutes. If no current memory the search back and forth limited by (Search * 4 minutes). ' If no search memory then flag with Azimuth_Memory 0. ' Get_Path is based on binary radians, Brads 0-255, calculates 8 bit sun position to prevent fires from dish misalignment. ' flag indicates quadrant for ATN (arctangent). ' ' Safety more important than function. 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 previous_second VAR Byte 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 prior VAR work4.LOWBYTE north VAR work1 south VAR work2 east VAR work3 west VAR work4 dat2 VAR work.LOWBYTE dat1 VAR work.HIGHBYTE pointer VAR work1 azPast VAR work2.HIGHBYTE altPast VAR work2.LOWBYTE azFuture VAR work3.HIGHBYTE altFuture VAR work3.LOWBYTE declination VAR work4.LOWBYTE Azimuth_Memory VAR work5.LOWBYTE Altitude_Memory VAR work5.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 alt VAR work2 az VAR work3 h VAR work4 Azimuth_Path VAR work1.BYTE0 Altitude_Path VAR work1.BYTE1 lat CON 34 '47 degrees Tacoma = 33 brads ~round up~ 34 (binary radians 0-255 in circle) Scale CON $0C0 drift CON 4 Search CON 8 night CON 65500 twilight CON 20000 sun CON 700 sun2 CON 1400 sun_Gap CON 100 Azimuth_Gap CON 2 Altitude_Gap CON 4 ON_SUN CON 700 Too_Bright CON 20 lowZ CON 51 highZ CON 204 lowD CON 6 'degrees down highD CON 90 'degrees up rangeZ CON highZ-lowZ rangeD CON highD-lowD rateZ CON rangeZ*180/rangeD*2 '256/128 INVrateZ CON rangeD*128/180*256/rangeZ horizonQ CON lowD*rangeZ/rangeD horizonZ CON lowZ-horizonQ lowXY CON 14 highXY CON 237 due_south CON 127 rangeXY CON highXY-lowXY INVrateXY CON 65535/rangeXY danger CON 20 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 Start: GOSUB Get_Sensors GOSUB Get_Time GOSUB Get_Eyes GOSUB Get_Memory RUN 1 '**************************************************** Get_Sensors: 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 PUT 9, 1 'Azimuth sensor failure ########### Altitude_Stow = yes ENDIF 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 PUT 9, 2 'Altitude sensor failure ########### Azimuth_Stow = yes ENDIF 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 ' Reverse sensor statement RETURN '***************************************************** Get_Time: I2CIN cport,$d1,0,[sec,minute,hour,day,date,month,year] work4 = (hour.HIGHNIB*10+hour.LOWNIB)*60+(minute.HIGHNIB*10+minute.LOWNIB) #IF DebugMode #THEN DEBUG CRSRXY, 0,0,CLREOL, HEX2 month,"/",HEX2 date,"/",HEX2 year," day ",HEX2 day," ", HEX2 hour,":",HEX2 minute,":",HEX2 sec, " Time ",DEC work4 #ENDIF IF work4 < time OR time = 0 THEN ' New day. PUT 3, 0 solar_noon = 1 work = ((month.HIGHNIB*10+month.LOWNIB - 1) * 30) + (date.HIGHNIB*10+date.LOWNIB) '360 days/year close enough work = 284 + work ' D=23.45 degrees*sin(360 degrees*(284+N)/365)= Declination IF work > 365 THEN work = work - 365 work = work * 100 / 365 * 255 / 100 work = SIN work sign = work.BIT15 'd = ABS work * 16.675 / 127 work = (ABS work */ 4269) / 127 '23.45/360*256=16.675 16.675*256=4269 IF sign THEN work = 255 - work d = work ' Declination byte in brads used in Path routines. ENDIF IF Time = work4 AND (sec.HIGHNIB*10+sec.LOWNIB)= previous_second THEN PUT 9, 3 'Time failure ########### Azimuth_Stow = yes Altitude_Stow = yes ENDIF previous_second = (sec.HIGHNIB*10+sec.LOWNIB) IF work4.BIT2 <> Time.BIT2 THEN read_flag = 0 write_flag = 1 ENDIF sign = 0 IF time AND work4.BIT0 = Time.BIT0 THEN zero ' Else new minute sign = 1 IF dwell THEN dwell = dwell - 1 IF Azimuth_Motor THEN Azimuth_Motor = Azimuth_Motor + 1 IF Altitude_Motor THEN Altitude_Motor = Altitude_Motor + 1 IF Azimuth_Motor = 4 THEN GET 1, dat1 IF ABS(Azimuth_sensor - dat1) <= Azimuth_Gap THEN PUT 9, 4 'Azimuth motor failed to move ########### Altitude_Stow = yes ELSE Azimuth_Motor = 1 ENDIF ENDIF IF Altitude_Motor = 3 THEN GET 2, dat2 IF ABS(Altitude_sensor - dat2) <= Altitude_Gap AND Altitude_sensor < (highZ - (Altitude_Gap * 2)) AND Altitude_sensor > (lowZ + (Altitude_Gap * 2)) THEN PUT 9, 5 'Altitude motor failed to move (check limit switches) ########### Azimuth_Stow = yes ELSE Altitude_Motor = 1 ENDIF ENDIF zero: Time = work4 IF sign THEN GOSUB Get_Path RETURN '*************************************************** Get_Eyes: north = 0 south = 0 east = 0 west = 0 FOR Smooth = 0 TO 7 HIGH North_eye PAUSE 1 RCTIME North_eye, 1, work IF work = 0 THEN work = 65535 north = north + (work / 8) HIGH South_eye PAUSE 1 RCTIME South_eye, 1, work IF work = 0 THEN work = 65535 south = south + (work / 8) HIGH East_eye PAUSE 1 RCTIME East_eye, 1, work IF work = 0 THEN work = 65535 east = east + (work / 8) HIGH West_eye PAUSE 1 RCTIME West_eye, 1, work IF work = 0 THEN work = 65535 west = west + (work / 8) NEXT #IF DebugMode #THEN 'DEBUG CRSRXY,10,11,"West/East",CRSRX,20,"North/South" DEBUG CRSRXY,0,12,"Eye",CLREOL,CRSRX,10,DEC west, CRSRX,20,DEC north DEBUG CRSRXY,0,13,"Eye",CLREOL,CRSRX,10,DEC east, CRSRX,20,DEC south DEBUG CRSRXY,0,14,"Sight",CLREOL,CRSRX,10,SDEC west-east, CRSRX,20,SDEC north-south #ENDIF IF north < Too_Bright OR south < Too_Bright OR east < Too_Bright OR west < Too_Bright THEN PUT 9, 6 'Eye failure ########### Altitude_Stow = yes Azimuth_Stow = yes ENDIF cloudy = yes IF (west < sun OR east < sun OR (west 900 OR (west > twilight OR east > twilight OR north > twilight OR south > twilight)) THEN RETURN IF west > night OR east > night OR north > night OR south > night THEN dark = yes RETURN ENDIF dark = no IF cloudy THEN RETURN IF west < east THEN azimuth_command = right ELSE azimuth_command = left IF north < south THEN altitude_command = up ELSE altitude_command = down IF ABS(west - east) < sun_gap THEN azimuth_go = stop_ ELSE azimuth_go = go IF ABS(north - south) < sun_gap THEN altitude_go = stop_ ELSE altitude_go = go IF north < on_sun AND south < on_sun AND west < on_sun AND east < on_sun AND azimuth_go = stop_ AND altitude_go = stop_ THEN Focus = 1 ELSE Focus = 0 GET 3, work2 'Path work3 =((Azimuth_Sensor-lowXY)*/INVrateXY)+1 IF azimuth_go = go AND ABS(work2 - work3) > danger THEN IF (work2 > work3 AND azimuth_command = left) OR (work2 < work3 AND azimuth_command = right) THEN 'IF Altitude_Stow = no AND Azimuth_Stow = no THEN PUT 9, 7 'Azimuth eye/sensor/clock error. Check all three. ########### 'ENDIF altitude_stow = yes RETURN ENDIF ENDIF GET 4, work2 'Path work3 =((Altitude_Sensor-horizonZ)*/INVrateZ)+1 IF Altitude_go = go AND ABS(work2 - work3) > danger/2 THEN IF (work2 > work3 AND Altitude_command = down) OR (work2 < work3 AND Altitude_command = up) THEN 'IF Altitude_Stow = no AND Azimuth_Stow = no THEN PUT 9, 8 'Altitude eye/sensor/clock error. Check all three. ########### 'ENDIF Azimuth_Stow = yes RETURN ENDIF ENDIF IF Focus THEN IF Azimuth_sensor = due_south AND solar_noon THEN IF ABS(Time - 720) > 40 THEN PUT 9, 9 'Solar_noon clock/position error. ########### altitude_stow = yes GOTO Time_out ENDIF I2COUT cport, $D0, 1, [$00, $12]:Time=720 ' Long term autonomy solar noon clock reset. solar_noon = 0 ENDIF IF write_flag THEN Pointer = Time / 4 * 4 declination = d + 128 WRITE pointer, Azimuth_sensor, Altitude_sensor, declination write_flag = 0 #IF DebugMode #THEN DEBUG 7 #ENDIF ENDIF ENDIF Time_out: RETURN '****************************************************** Get_Memory: IF dark OR read_flag THEN RETURN dat1 = 0 dat2 = 0 read_flag = 1 work4.HIGHBYTE = d + 128 Pointer = Time / 4 * 4 READ pointer, Azimuth_Memory, Altitude_Memory, declination IF ABS(declination - work4.HIGHBYTE) > drift THEN Search_back GOTO Remember Search_back: dat1 = dat1 + 1 IF dat1 > Search THEN no_memory Pointer = time / 4 * 4 - (4 * dat1) READ pointer, AzPast, AltPast, declination IF ABS(declination - work4.HIGHBYTE) > drift THEN Search_back Search_forward: dat2 = dat2 + 1 IF dat2 > Search THEN no_memory Pointer = time / 4 * 4 + (4 * dat2) READ pointer, AzFuture, AltFuture, declination IF ABS(declination-work4.HIGHBYTE) > drift THEN Search_forward Azimuth_Memory = (ABS(azFuture - azPast) * dat1)/(dat1+dat2) Altitude_Memory = (ABS(altFuture-altPast) * dat1)/(dat1+dat2) IF azPast > azFuture THEN Azimuth_Memory = azPast - Azimuth_Memory ELSE Azimuth_Memory = azPast + Azimuth_Memory IF altPast > altFuture THEN Altitude_Memory = altPast - Altitude_Memory ELSE Altitude_Memory = altPast + Altitude_Memory GOTO Remember no_memory: Azimuth_Memory = 0 Remember: PUT 7, Azimuth_Memory PUT 8, Altitude_Memory RETURN '****************************************************** Get_path: IF time < 120 OR time > 1320 THEN RETURN ' N=days H=+/-hours*15 degrees Day of year Hours from solar noon ' D=23.45 degrees*sin(360*(284+N)/365 Declination ' ALT=arcsin(sinL*sinD+cosL*cosD*cosH) ' AZ=arcsin(cosD*sinH/cosALT) 'h = timex*15/60*128/180-127 ' 15 degrees/hour = 0.17777 brads/minute 'h = timex * 17.777 / 100 - 127 h = (time */ 4550) / 100 - 127 '17.777*256=4550 IF h.BIT15 = 1 THEN h = 255 + h alt = ((SIN lat) * ABS(SIN d)) / 127 work = SIN d IF work.BIT15 = 1 THEN alt = -alt work = COS h sign = work.BIT15 work = (ABS(COS h)) * (COS d) / 127 * (COS lat) / 127 IF sign THEN work = -work alt = alt + work work = SQR(16129 - (alt * alt)) Altitude_Path = work ATN alt work = COS Altitude_Path az = ((COS d) * (SIN h)) sign = work.BIT15 ^ az.BIT15 az = ABS az / ABS work IF sign THEN az = -az work = SQR(16129 - (az * az)) Azimuth_Path = (work ATN az) + 128 prior = (-work ATN az) + 128 IF ABS(Azimuth_Path - prior) < 10 THEN IF time < 720 THEN flag = 0 ELSE flag = 1 ENDIF IF flag THEN Azimuth_Path = prior GET 3, prior IF prior > Azimuth_Path THEN Azimuth_Path = prior PUT 3, Azimuth_Path PUT 4, Altitude_Path RETURN