'********************************************** '* Joystick Channel debug with transmit * '********************************************** 'Last update 110422 'code compiled by Johan Ancker 2009 inspired by code for ad channel debug by Bryan Kobe, June, 2007. 'This code is used for addressing and accessing all 16 channels on two MCP 3208 ad converters 'It will address and debug all channels, returning the value 0-4095. 'It will also send decoded data. 'Diode toggle taken away!!! 'See end of file for terms of use. CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 ' data for Din to start conversion for each ADx channel AD0 = %11000 AD1 = %11001 ' Note: See Datasheet MUX Addressing for more details AD2 = %11010 ' Bit4 =Start Bit, Bit3 =SGL/DIF, Bit2 =D2, Bit1 =D1, Bit0 =D0 AD3 = %11011 AD4 = %11100 AD5 = %11101 AD6 = %11110 AD7 = %11111 ' pin configuration clk = 0 cs = 3 dataout = 2 ' (= Din on AD-conv.) datain = 1 ' (= Dout on AD-conv.) clk2 = 4 cs2 = 7 dataout2 = 6 ' (= Din on AD-conv2.) datain2 = 5 ' (= Dout on AD-conv2.) 'Carriage Return aso in PC-debug CR = 13 FF = 12 LF = 10 Space = " " 'Synksignal. sync = 255 OBJ bs2 : "BS2_Functions_Fast" debug : "pc_debug" num : "Numbers" sanda : "sandadata" VAR byte ADC ' Analog to Digital Channel Din Value. Set using CON values. byte ADC2 ' Analog to Digital Channel Din Value (AD2). Set using CON values. long datar ' 12 Bit return value from conversion long datar2 ' 12 Bit return value from conversion from AD2. Long stack1[100] ' Stack for 2nd BS2 Cog long zero long accx long accy long accz long accxm long accym long acczm long j1accx long j1accy long j1accz long j2accx long j2accy long j2accz long j1xm long j1ym long j1zm long j2xm long j2ym long j2zm long PHI ' roll *100 radians long PHIj1 ' rollj1 *100 rad long PHIj2 ' rollj2 *100 rad long TATA ' pitch *100 rad long TATAj1 ' pitchj1 *100 rad long TATAj2 ' pitchj2 *100 rad long tilt ' pitch with scaling posibility long tiltj1 long tiltj2 long roll ' roll with scaling posibility long rollj1 long rollj2 long p ' 100*accy/accz allt j1ym,j1zm allt j2ym,j2zm (reused) will be used in Taylor computing of PHI long q ' 100*accx/accz allt j1xm,j1zm allt j2ym,j2zm (reused) will be used in Taylor computing of TATA long f ' 100*accy/accz or 100*accz/accy momentarily not used. long g byte boink2 long MH ' Motor command, forward/backward motor stern. long MV ' Motor command, forward/backward motor port. long MVH ' Motor command, upp/down motor stern. long MVV ' Motor command, upp/down motor port. PUB main | i dira[cs]~~ 'set Chip Select to output outa[cs] := 1 'set Chip Select High dira[clk]~~ 'set clk pin to output outa[clk]:=0 'set clock pin low dira[datain]~ 'set data in pin to an input dira[dataout]~~ 'set data out pin to an output dira[cs2]~~ 'set Chip Select2 to output outa[cs2] := 1 'set Chip Select2 High dira[clk2]~~ 'set clk2 pin to output outa[clk2]:=0 'set clock2 pin low dira[datain2]~ 'set data2 in pin to an input dira[dataout2]~~ 'set data2 out pin to an output Num.Init sanda.start(17,16,0,9600) ' start terminal, will (see obj sanda) use pins 17 (recieve) and 16 (send) debug.start(9600) ' start terminal, will by default (see obj debug) use pins 30 and 31 debug.str(string(FF, "Joyst Channel Debug med 9600 baud", CR, LF, LF)) debug.crlf waitcnt(2_000_000 + cnt) start_bs2 'Start bs2 functions repeat repeat i from 0 to 10 zero:= 2048*5 accx:= GetADC(0)*5 accy:= GetADC(1)*5 accz:= GetADC(2)*5 j1accx:= GetADC2(3)*5 j1accy:= GetADC2(4)*5 j1accz:= GetADC2(5)*5 j2accx:= GetADC2(0)*5 j2accy:= GetADC2(1)*5 j2accz:= GetADC2(2)*5 accx:= (accx-zero + 250)*-1 accy:= (accy-zero + 665)*-1 accz:= (accz-zero)*1 j1accx:= (j1accx-zero + 50)*-1 j1accy:= (j1accy-zero - 150)*-1 j1accz:= (j1accz-zero)*1 j2accx:= (j2accx-zero + 700)*-1 j2accy:= (j2accy-zero - 300)*-1 j2accz:= (j2accz-zero)*1 accxm:= accx+accxm accym:= accy+accym acczm:= accz+acczm j1xm:= j1accx+j1xm j1ym:= j1accy+j1ym j1zm:= j1accz+j1zm j2xm:= j2accx+j2xm j2ym:= j2accy+j2ym j2zm:= j2accz+j2zm waitcnt(50_000 + cnt) accxm:= accxm/11 accym:= accym/11 acczm:= acczm/11 j1xm:= j1xm/11 j1ym:= j1ym/11 j1zm:= j1zm/11 j2xm:= j2xm/11 j2ym:= j2ym/11 j2zm:= j2zm/11 ' **************** Compute for fixed xaccm!! ********************************************* p:= 100*accym/acczm q:= 100*accxm/acczm PHI:= p-p*p*p/30000+p*p*p*p*p/500000000 TATA:= q-q*q*q/30000+q*q*q*q*q/500000000 tilt:= TATA roll:= PHI ' **************** Compute for moving xaccm1!! ********************************************* p:= 100*j1ym/j1zm q:= 100*j1xm/j1zm PHIj1:= p-p*p*p/30000+p*p*p*p*p/500000000 rollj1:= PHIj1 TATAj1:= q-q*q*q/30000+q*q*q*q*q/500000000 tiltj1:= TATAj1 ' **************** Compute for moving xaccm2!! ********************************************* p:= 100*j2ym/j2zm q:= 100*j2xm/j2zm PHIj2:= p-p*p*p/30000+p*p*p*p*p/500000000 rollj2:= PHIj2 TATAj2:= q-q*q*q/30000+q*q*q*q*q/500000000 tiltj2:= TATAj2 ' ********************The following is for debug only!************************************ IF boink2 == 4 ' boink2 = 5 dont write! debug.crlf debug.str(string("accxm, accym, acczm=")) debug.str(num.ToStr(accxm, Num#dec)) debug.str(string(" ")) debug.str(num.ToStr(accym, Num#dec)) debug.str(string(" ")) debug.str(num.ToStr(acczm, Num#dec)) debug.str(string(" ")) debug.crlf debug.str(string("Roll*100=")) debug.str(num.ToStr(roll, Num#dec)) debug.str(string(" ")) debug.str(string("Tilt*100=")) debug.str(num.ToStr(tilt, Num#dec)) debug.str(string(" ")) debug.crlf debug.str(string("j1xm, j1ym, j1zm=")) debug.str(num.ToStr(j1xm, Num#dec)) debug.str(string(" ")) debug.str(num.ToStr(j1ym, Num#dec)) debug.str(string(" ")) debug.str(num.ToStr(j1zm, Num#dec)) debug.str(string(" ")) debug.crlf debug.crlf debug.str(string("j2xm, j2ym, j2zm=")) debug.str(num.ToStr(j2xm, Num#dec)) debug.str(string(" ")) debug.str(num.ToStr(j2ym, Num#dec)) debug.str(string(" ")) debug.str(num.ToStr(j2zm, Num#dec)) debug.str(string(" ")) debug.crlf debug.crlf debug.str(string("MV, MH, MVV, MVH=")) debug.str(num.ToStr(MV, Num#dec)) debug.str(string(" ")) debug.str(num.ToStr(MH, Num#dec)) debug.str(string(" ")) debug.str(num.ToStr(MVV, Num#dec)) debug.str(string(" ")) debug.str(num.ToStr(MVH, Num#dec)) debug.str(string(" ")) debug.crlf debug.str(string("Sync=")) debug.str(num.ToStr(sync, Num#dec)) debug.str(string(" ")) ' ****************************Ready writing!**************************************** accxm:= 0 ' Zeroing of mean values. accym:= 0 acczm:= 0 j1xm:= 0 j1ym:= 0 j1zm:= 0 j2xm:= 0 j2ym:= 0 j2zm:= 0 ' ********************Count up of write trigging (for debug)!!*********************** boink2 := boink2+1 IF boink2> 4 boink2:=0 waitcnt(50_000 + cnt) ' **************************************************************************************** rollj1 := rollj1-roll ' Take away box tilt! tiltj1 := tiltj1-tilt ' Take away box tilt! rollj2 := rollj2-roll ' Take away box tilt! tiltj2 := tiltj2-tilt ' Take away box tilt! MH:= 8*rollj1 + 6*tiltj1 MV:= 8*rollj1 - 6*tiltj1 MVH:= 8*rollj2 + 6*tiltj2 MVV:= 8*rollj2 - 6*tiltj2 IF MH> 125 'limiting max range (n.b. space important at > and 125 'limiting max range (n.b. space important at > and 125 'limiting max range (n.b. space important at > and 125 'limiting max range (n.b. space important at > and