VAR ' For LCD:n: long stack[60] word cog word sync2 long xm long ym ' long zm word Vecm long vinkel ' result vinkel2+vinkel2high word vinkel2 ' yaw computed one more time *100 i deg ranging from zero to 360 word vinkel2high ' high bit vinkel 2 long tiltvisa ' pitch i degrees with reversed sign. long rollvisa ' rollangle in degrees word djup word displaystatus ' Uppräkning long runtime byte displight ' 0 or 1 dependent of if switch set off or on!! byte rantarunt ' send "no signal" to screen. byte rantax byte rantay byte lockswitch byte locklock ' locks lock value long headlock ' value of heading that is locked on long headdiff ' difference between heading and headlock long headdiffold ' 1 recursion old figure long headdiffoldold ' 2 recursions old figure long headdiffa ' headdiff multiplied by factor (have to be separated from headdiff because of recursion) byte depthlockswitch ' max length of variables ae 30 char so this is ok! byte depthlocklock ' locks lock value long depthlock ' value of heading that is locked on long depthdiff ' difference between heading and headlock long depthdiffold ' 1 recursion old figure long depthdiffoldold ' 2 recursions old figure long depthdiffa ' headdiff multiplied by factor (have to be separated from headdiff because of recursion) byte tickyr ' tick right byte tickyl ' tick left long ticky ' ticksum left and right byte tickzu ' tick upp byte tickzd ' tick down long tickz ' ticksum up and down OBJ duplex : "FullDuplexSerial_mini" LCD : "Debug_Lcd_min_ver" PUB start : okay '' Starts recieve pub (object) '' -- returns true if all parameters okay stop okay := cog := cognew(recieve, @stack) + 1 'Shall be cognew here!!! PUB stop '' Stops recieve object -- frees the pin (floats) if cog cogstop(cog~ - 1) PUB recieve : vinkla | i duplex.start(17,10,0,9600) ' Start full duplex serial. Recieve at 17, send at pin 10! waitcnt(800_000 + cnt) ' Wait a little! duplex.tx(126) ' send 126 at an early stadium (to adjy) waitcnt(20_000_000 + cnt) ' Wait a little! 'start LCD, will (see obj Debug_Lcd) use pin 11. Lcd.start(11,9600,4) ' start LCD, will (see obj Debug_Lcd) use pin 11 Lcd.backLight(true) Lcd.cls ' Clear the LCD LCD.home LCD.custom(0, @Bullet) ' create custom character 0 LCD.custom(1, @LC1) ' create custom character 1 LCD.custom(2, @LC2) ' create custom character 2 LCD.custom(3, @LC3) ' create custom character 3 LCD.custom(4, @LC4) ' create custom character 4 LCD.custom(5, @LC5) ' create custom character 5 LCD.custom(6, @LC6) ' create custom character 6 '****************************Perform startup sekvence.************************************************* startup_sequence LCD.cls 'Clear screen '**************************** Start reading data. ************************************************ repeat sync2:=duplex.rx If sync2==255 xm:=duplex.rx ym:=duplex.rx ' zm:=duplex.rx 'Momentarily not shown. Vecm:=duplex.rx Vinkel2:=duplex.rx Vinkel2high:=duplex.rx tiltvisa:=duplex.rx rollvisa:=duplex.rx djup:=duplex.rx xm := xm - 100 'Decondition ym := ym - 100 Vinkel:=Vinkel2+Vinkel2high 'Decondition tiltvisa:= (tiltvisa-100) ' Decondition rollvisa:= (rollvisa-100) LCD.home LCD.str(string("xm=")) LCD.decf(xm,3) LCD.str(string(" Runtime")) LCD.decf(runtime/ 5,3) ' LCD.str(string(" ym=")) ' LCD.decf(ym,3) LCD.gotoxy(0,1) LCD.str(string(" head=")) LCD.decf(Vinkel,4) LCD.str(string(" degres")) LCD.gotoxy(0,2) LCD.str(string("roll=")) LCD.decf(rollvisa,2) LCD.str(string(" tilt=")) LCD.decf(tiltvisa,2) LCD.gotoxy(0,3) LCD.str(string(" Vecm=")) LCD.decf(Vecm,3) LCD.str(string(" ym=")) LCD.decf(ym,3) waitcnt(12_000_000 + cnt) displaystatus:= displaystatus + 1 ' Count up displaystatus. react at 100. If displaystatus > 48 Runtime:= runtime + 1 LCD.cls 'Clear screen displaystatus:= 0 displight := ina[13] If displight <> 0 Lcd.backLight(false) Else Lcd.backLight(true) ELSE waitcnt(3_000_000 + cnt) displaystatus:= displaystatus + 1 ' Count up displaystatus. react at 100. If displaystatus > 100 LCD.cls 'Clear screen If displight == 0 and rantarunt == 3 Lcd.backLight(true) Else Lcd.backLight(false) rantax := 5 * rantarunt rantay := rantay + rantarunt if rantax > 12 rantax := 0 If rantay > 3 rantay := 0 LCD.gotoxy(rantax,rantay) LCD.str(string("No signal")) waitcnt(100_000_000 + cnt) Lcd.backLight(false) 'Shut off. LCD.cls 'Clear screen rantarunt := rantarunt + 1 If rantarunt > 3 rantarunt := 0 displight := ina[13] 'Read here, react next cykle!!! displaystatus:= 0 '**********************************Check y-tick****************************************************** tickyr := ina[18] tickyl := ina[19] ticky := ticky + tickyr - tickyl '*****************************Compute adjy!********************************************************** lockswitch := ina[20] If lockswitch == false locklock := 0 headdiff := 0 headdiffold := 0 headdiffoldold := 0 If lockswitch == 1 and locklock == false locklock := 1 headlock := vinkel If lockswitch == 1 headdiffoldold := headdiffold headdiffold := headdiff headdiff := vinkel - headlock ' Compute offset If headdiff > 300 headdiff := headdiff - 360 ' 360 fix! If headdiff < -300 headdiff := headdiff + 360 headdiff := (2 * headdiff + 2 * headdiffold + headdiffoldold) / 5 ' Soften!! headdiffa := 4 * ticky + 4 * headdiff 'Can not reuse headdiff here. It is recursed!! IF headdiffa > 60 'Limit max command headdiffa := 60 IF headdiffa < -60 headdiffa := -60 '(Max 125 will make values less than 256.) '*****************************beräkna adjz!!!******************************************************* depthlockswitch := ina[24] ' 24 switch of your choise!!! If depthlockswitch == false depthlocklock := 0 depthdiff := 0 depthdiffold := 0 depthdiffoldold := 0 If depthlockswitch == 1 and depthlocklock == false depthlocklock := 1 depthlock := djup ' xx. If depthlockswitch == 1 depthdiffoldold := depthdiffold depthdiffold := depthdiff depthdiff := djup - depthlock ' compute difference depthdiff := (2 * depthdiff + 2 * depthdiffold + depthdiffoldold) / 5 ' Soften!! depthdiffa := 4 * tickz + 4 * depthdiff 'headdiff cannot be directly reused. It is recursed!! IF depthdiffa > 60 'Max limit depthdiffa := 60 IF depthdiffa < -60 depthdiffa := -60 '(Max 125 will make values less than 256.) '***************************sänd ut headdiff!!************************************************************* duplex.tx(headdiffa + 126) ' Send result to main cog and condition!! '************************************************************************************************************* PRI startup_sequence | i LCD.cls LCD.gotoxy(1,0) LCD.str(string("Name of")) LCD.gotoxy(3,1) LCD.str(string("your")) LCD.gotoxy(9,1) ' Go to row 1, pos 9! LCD.putc(1) ' display custom LC1 character LCD.putc(2) ' display custom LC2 character LCD.putc(3) ' display custom LC3 character LCD.gotoxy(9,2) ' Go to row 2, pos 9! LCD.putc(4) ' display custom LC4 character LCD.putc(5) ' display custom LC5 character LCD.putc(6) ' display custom LC6 character LCD.str(string(" project")) Repeat i from 0 to 5 LCD.gotoxy(15,0) LCD.decf((5-i),2) LCD.gotoxy(0,3) LCD.putc(0) ' display custom bullet character LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) waitcnt(20_000_000 + cnt) LCD.gotoxy(1,3) LCD.putc(0) ' display custom bullet character LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) waitcnt(20_000_000 + cnt) LCD.gotoxy(2,3) LCD.putc(0) ' display custom bullet character LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) LCD.str(string(" ")) LCD.putc(0) waitcnt(20_000_000 + cnt) DAT Bullet byte $00, $04, $0E, $1F, $0E, $04, $00, $00 LC1 byte $18, $0C, $0B, $08, $08, $04, $09, $12 ' Little cat LC2 byte $00, $04, $1F, $00, $00, $00, $11, $0A ' Little cat LC3 byte $03, $06, $1A, $02, $02, $04, $12, $09 ' Little cat LC4 byte $13, $18, $10, $0A, $09, $04, $06, $01 ' Little cat LC5 byte $11, $04, $0E, $00, $00, $1F, $04, $1F ' Little cat LC6 byte $19, $03, $01, $0A, $12, $04, $0C, $10 ' Little cat {{ -------------------------------------------------------------------------------------------------------------------------------- ! TERMS OF USE: MIT License ! !------------------------------------------------------------------------------------------------------------------------------! !Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation ! !files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, ! !modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software! !is furnished to do so, subject to the following conditions: ! ! ! !The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.! ! ! !THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE ! !WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR ! !COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ! !ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ! !------------------------------------------------------------------------------------------------------------------------------! }}