'********************************************************************************************** '* Joystick Accelerometer decode with transmit and recieve, do also see object recieve! * '********************************************************************************************** 'Last update 110422. 'Base of code compiled and revised by Bryan Kobe June, 2007, changed and enhanced by Johan Ancker 2009, 2010 and 2011. 'This code is used for addressing and accessing all 8 channels on the MCP 3208 (Brian Kobe base). 'It will address and debug all 8 channels, returning the value 0-4095. 'It will also send out decoded data. (Enhancement by Johan Ancker) 'It will also recieve data from outboard device such as vehicle etc. Recieved data 'is presented on a display.(Enhancement by Johan Ancker) 'It will also perform cetrtain auto pilot tasks dependent on your control switch settings. Vehicle data are used. '(Enhancement by Johan Ancker) 'I started on it 090916 'Tactics: Use pin 10 to send the adjust heading value (headdiffa) to pin 12 used for recive in sanda object on this cog. ' Use a 22K resistor at the connection. 'New gen change: LED at new position at pin 10 monitoring the pasover from pin 10 to 12. This means that PUB toggle was taken away. ' Also lighter startup seqence. 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.) displightpin = 13 ' (= input for setting displaylight on or off) 'Carriage Return aso in PC-debug CR = 13 FF = 12 LF = 10 Space = " " 'Synksignal. sync = 255 OBJ bs2 : "BS2_Functions_Fast" num : "Numbers" sanda : "sandadata" reci : "recieve7" 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 in radians long PHIj1 ' rollj1 *100 in rad long PHIj2 ' rollj2 *100 in rad long TATA ' pitch *100 in rad long TATAj1 ' pitchj1 *100 in rad long TATAj2 ' pitchj2 *100 in rad long tilt ' pitch in degrees with reversed sign. long tiltj1 long tiltj2 long roll ' roll-angle in deg long rollj1 ' roll-angle j1 in deg long rollj2 ' roll-angle j2 in deg 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 will be used in Taylor computing of PHI in another way. long g long MH ' Command to x-engine right long MV ' Command to x-engine left long MVH ' YZ engine command right long MVV ' YZ engine command left byte mode ' mode=0: Full speed, no lights; 1=low speed no lights, 2=full speed, left light ' 3=low speed left light; 4=full speed right light; 5= low speed right light; ' 6=full speed both lights; 7=low speed both lights! Mode is sum of speed2 and light2. byte speed ' Toggle between 0 and 1 for speed mode byte speed2 ' After toggle byte light ' Toggle light mode byte light2 ' After toggle, values 0,1,2 and 3. byte tickxf ' tick forward byte tickxb ' tick backward long tickx ' ticksum back and forward long adjy ' adjust y 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 dira[displightpin]~ 'set displaylight pin to an input. dira[8]~ 'set pin 8 (speed switch) to an input. dira[9]~ 'set pin 9 (vehicle light toggler switch) to an input. dira[18]~ 'set pin 18 (tickyr switch) to an input. dira[19]~ 'set pin 19 (tickyl switch) to an input. dira[20]~ 'set pin 20 (lock heading switch) to an input. dira[21]~ 'set pin 21 (lock heading to previous switch) to an input. dira[22]~ 'set pin 22 (tickxf switch) to an input. dira[23]~ 'set pin 23 (tickxb switch) to an input. Num.Init sanda.start(12,16,0,9600) ' start bs2 send and recive functions, will (see obj sanda) use pins 12 (recieve) ' and 16 (send). ' N.B! In simple verion pin 17 can be used as recieve here! reci.start 'Start recieve cog waitcnt(2_000_000 + cnt) start_bs2 'Start bs2 functions repeat repeat i from 0 to 15 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(34_375 + cnt) ' 50_000*11/16. Used 11 loops earlier with 50_000 clockcykles wait cnt. accxm:= accxm/16 accym:= accym/16 acczm:= acczm/16 j1xm:= j1xm/16 j1ym:= j1ym/16 j1zm:= j1zm/16 j2xm:= j2xm/16 j2ym:= j2ym/16 j2zm:= j2zm/16 ' **************** Compute for box fix accm!! ********************************************* p:= 100*accym/acczm q:= 100*accxm/acczm PHI:= p-p*p*p/30000+p*p*p*p*p/500000000 'Former phi computation!!! TATA:= q-q*q*q/30000+q*q*q*q*q/500000000 tilt:= TATA roll:= PHI ' **************** Compute for moving accm1!! ********************************************* p:= 100*j1ym/j1zm q:= 100*j1xm/j1zm PHIj1:= p-p*p*p/30000+p*p*p*p*p/500000000 'Former phi computation!!! rollj1:= PHIj1 TATAj1:= q-q*q*q/30000+q*q*q*q*q/500000000 tiltj1:= TATAj1 ' **************** Compute for moving accm2!! ********************************************* p:= 100*j2ym/j2zm q:= 100*j2xm/j2zm PHIj2:= p-p*p*p/30000+p*p*p*p*p/500000000 'Former phi computation!!! rollj2:= PHIj2 TATAj2:= q-q*q*q/30000+q*q*q*q*q/500000000 tiltj2:= TATAj2 ' ***************************************************************************************** accxm:= 0 ' Zeroing mean values. accym:= 0 acczm:= 0 j1xm:= 0 j1ym:= 0 j1zm:= 0 j2xm:= 0 j2ym:= 0 j2zm:= 0 ' **************************************Check mode:****************************************** speed := ina[8] speed2 := speed2 + speed If speed2 > 1 speed2 := 0 light := ina[9] light2 := light2 + light ' Accumulate button commands. If light2 > 3 light2 := 0 mode := 2 * light2 + speed2 'compute mode! '**********************************Check tick-x***************************************************** tickxf := ina[22] tickxb := ina[23] tickx := tickx + tickxf - tickxb ' ********************************Adjust heading!*************************************** adjy := sanda.in - 126 'acount for adjustment y och and decondition. If adjy == -126 adjy := 0 'cancel out start up problems. waitcnt(50_000 + cnt) ' **************************************************************************************** rollj1 := rollj1-roll ' Cancel out box position! tiltj1 := tiltj1-tilt ' Cancel out box position! rollj2 := rollj2-roll ' Cancel out box position! tiltj2 := tiltj2-tilt ' Cancel out box position! MH:= 7*rollj1 + 4*tiltj1 - adjy + tickx 'Right x motor command, adjy justify heading! MV:= 7*rollj1 - 4*tiltj1 + adjy + tickx MVH:= 7*rollj2 + 5*tiltj2 MVV:= 7*rollj2 - 5*tiltj2 IF MH> 125 'Max command limiter (N.B. space important 'at > and < signs !!!) MH:= 125 IF MH< -125 MH:= -125 IF MV> 125 'Max command limiter (N.B. space important 'at > and < signs !!!) MV:= 125 IF MV< -125 MV:= -125 IF MVH> 125 'Max command limiter (N.B. space important 'at > and < signs !!!) MVH:= 125 IF MVH< -125 MVH:= -125 IF MVV> 125 'Max command limiter (N.B. space important 'at > and < signs !!!) MVV:= 125 IF MVV< -125 MVV:= -125 MH:= MH+127 'MH with value always positive MV:= MV+127 'MV with value always positive MVH:= MVH+127 'MVH with value always positive MVV:= MVV+127 'MVV with value always positive waitcnt(200_000 + cnt) ' ***************************************************************************************************************** ' Utsändning: sanda.out(sync) 'sync recieved at opposite side, sync=255 as specified in con sanda.out(MH) sanda.out(MV) sanda.out(MVH) sanda.out(MVV) sanda.out(mode) 'send mode info! PUB Start_bs2 bs2.start (31,30) ' Initialize BS2 Object timing, Rx and Tx pins (have changed to 31 o 30 here!!!) PRI GetADC( chan ) : value if (chan == 0) ADC := AD0 if (chan == 1) ADC := AD1 if (chan == 2) ADC := AD2 if (chan == 3) ADC := AD3 if (chan == 4) ADC := AD4 if (chan == 5) ADC := AD5 if (chan == 6) ADC := AD6 if (chan == 7) ADC := AD7 datar := write(ADC) 'write MUX Address to start conversion for ADC channel and set result to the datar value return datar PRI write( ADCaddr ) : ADC_value outa[cs] := 0 'set Chip Select LOW, activating the ADC chip bs2.SHIFTOUT(dataout, clk, ADCaddr, BS2#MSBFIRST, 5) 'shift out the addressing byte to the ADC via DI pin on ADC ADC_value := bs2.SHIFTIN(datain, clk, BS2#MSBPOST, 13) 'shift in the byte from the ADC via the DO pin on the ADC 'the first byte is a start bit, to initiate reading sequence outa[cs]:=1 'set Chip Select HIGH, de-activating the ADC chip return ADC_value 'return value of the ADC PRI GetADC2( chan ) : value if (chan == 0) ADC2 := AD0 if (chan == 1) ADC2 := AD1 if (chan == 2) ADC2 := AD2 if (chan == 3) ADC2 := AD3 if (chan == 4) ADC2 := AD4 if (chan == 5) ADC2 := AD5 if (chan == 6) ADC2 := AD6 if (chan == 7) ADC2 := AD7 datar2 := write2(ADC2) 'write MUX Address to start conversion for ADC channel and set result to the datar2 value return datar2 PRI write2( ADCaddr2 ) : ADC_value2 outa[cs2] := 0 'set Chip Select LOW, activating the ADC chip bs2.SHIFTOUT(dataout2, clk2, ADCaddr2, BS2#MSBFIRST, 5) 'shift out the addressing byte to the ADC via DI pin on ADC ADC_value2 := bs2.SHIFTIN(datain2, clk2, BS2#MSBPOST, 13) 'shift in the byte from the ADC via the DO pin on the ADC 'the first byte is a start bit, to initiate reading sequence outa[cs2]:=1 'set Chip Select HIGH, de-activating the ADC chip return ADC_value2 'return value of the ADC {{ -------------------------------------------------------------------------------------------------------------------------------- ! 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. ! !------------------------------------------------------------------------------------------------------------------------------! }}