' {$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 12/09/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 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 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 previous_second VAR work.LOWBYTE 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 AM VAR work2 PM VAR work3 season VAR work5.LOWBYTE 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) 'lat CON 20 ' 27.5 degrees Ft. Pierce = 20 brads 'lat con 14 ' 18.5 degrees Mumbai Scale CON $0C0 drift CON 4 Search CON 8 night CON 65500 twilight CON 20000 sun CON 650 sun2 CON 1300 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 12 ' travel limit highXY CON 233 ' travel limit due_south CON 128 ' Polaris Star due_north CON 12 rangeXY CON due_south - due_north * 2 ' range/256 = sensors per brad INVrateXY CON 65535/rangeXY ' 256/range = brads per sensor danger CON 10 ' Pune India latitude 18.5 degrees (14 brads) ' IF d > lat (equatorial latitudes) then north tracking, flag_north ' run2.bsp program uses 30 days per month for day count, example: May 13 = 4*30+13 = 133 ' http://www.esrl.noaa.gov/gmd/grad/solcalc/ ' North sun > May 13 and < July 30 early_north CON 133 late_north CON 210 ' South sun not < September 23 and > March 19 all locations vernal_equinox CON 79 autuminal_equinox CON 263 ' 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 work5 = ((month.HIGHNIB*10+month.LOWNIB - 1) * 30) + (date.HIGHNIB*10+date.LOWNIB) '360 days/year close enough work = 284 + work5 ' 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 = 256 - work d = work ' Declination byte in brads used in Path routines. IF work5 > vernal_equinox AND work5 < autuminal_equinox THEN flag_south = 0 ELSE flag_south = 1 flag = 0 ENDIF 'IF work5 > early_north AND work5 < late_north THEN 'flag_north = 1 'flag = 1 'ELSE 'flag_north = 0 'ENDIF ENDIF GET 20, previous_second 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) PUT 20, previous_second 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 PUT 1, Azimuth_sensor ENDIF ENDIF IF Altitude_Motor = 4 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 PUT 2, Altitude_sensor ENDIF ENDIF zero: Time = work4 IF sign THEN GOSUB Get_Path RETURN '*************************************************** Get_Eyes: IF dark THEN RETURN 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 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 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 work2 > 50 OR ABS(d-lat) < 2 THEN overhead ' tropic crossover IF work2 > 50 THEN overhead ' if sun almost overhead then disable azimuth error testing GET 3, work2 'Path 'work3 =((Azimuth_Sensor-due_north)*/INVrateXY)+1 work3 = Azimuth_Sensor 'IF flag_north AND work2 > 128 THEN ' flag_north if declination > latitude ' work2 = due_north - ((256-work2) */ rangeXY) ' ELSE ' sensor travel start SSW W N E S W NNW end work2 = due_north + (work2 */ rangeXY) ' ENDIF 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 overhead: IF Focus THEN IF Azimuth_sensor = due_south AND solar_noon THEN 'IF (Azimuth_sensor = due_south OR Azimuth_sensor = due_north) 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 write_flag = 1 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 flag_run = 1 RUN 4 '****************************************************** Get_path: IF time < 60 OR time > 1380 THEN RETURN IF flag_north OR flag_south THEN known season = (d + 17) ' tracking Spring, Summer, sun in both north sky and south sky READ season * 4 + 1900, Word AM, Word PM IF time > AM AND time < PM THEN flag = 0 ELSE flag = 1 ' quadrant flag, 0 south 1 north ENDIF known: ' 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 ** 11650) - 128 ' 0.17777*65536=11650 IF h.BIT15 = 1 THEN h = 256 + 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)MAX 16129)) Altitude_Path = work ATN alt work = COS Altitude_Path az = ((COS d) * (SIN h)) sign = work.BIT15 ^ az.BIT15 az = (ABS az / ABS work) MAX 127 IF sign THEN az = -az work = SQR(16129 - (az * az)) IF flag THEN Azimuth_Path = (-work ATN az) + 128 ELSE Azimuth_Path = (work ATN az) + 128 ENDIF PUT 3, Azimuth_Path PUT 4, Altitude_Path IF Altitude_Path > 65 THEN dark = yes ELSE IF Altitude_Path > 1 THEN dark = no ' lock sunset and delay sunrise with gap of 2, could be zero. ENDIF ENDIF RETURN