Subject:
|
RE: Interfacing HB to PIC
|
Newsgroups:
|
lugnet.robotics.handyboard
|
Date:
|
Fri, 10 Jan 1997 03:21:24 GMT
|
Original-From:
|
Mike Ross <MROSS@OUTLAND.JSC.NASA.spamcakeGOV>
|
Viewed:
|
1621 times
|
| |
|
|
On Thu, 9 Jan 1997, Mike Lipsett wrote:
> Yo, Mike. If you've already done it, how about posting the code so the rest
> of us don't have to re-invent the wheel? It would be a service to the whole
> MB / HB community, which has been dancing around the SPI issue for a long time.
>
> Cheers,
> - Mike
Well, remember the code I have is in SBasic, not Interactive C. However,
it's not all that long so here it is. -mike
' sigproc.bas - written 12/96 for 68HC11E1 processor (Tcomp)
'
' pulls in 4 RC signals on the 4 IC interrupts, massages them as needed,
' and puts them out on the 4 OC interrupt lines.
' hooked to Vector 2X electronic compass via SPI port
' Copyright 1996 Mike Ross, CRT Innovations, Inc
' comments should be directed via email to:
' mross@ghg.net, mross@outland.jsc.nasa.gov
'
' assumptions:
' registers based at $1000
' all IC and OC (Port A) interrupts wired up as:
' IC1 feeds pulse1_length = steering
' IC2 feeds pulse2_length = pan
' IC3 feeds pulse3_length = tilt
' IC4 feeds pulse4_length = go (drive fore/aft)
' IC (fast) routines perform limiting and smoothing operations on incoming
' commands, as well as load TOC registers to drive commands out.
' Main (slow) loop computes heading correction factors to be added into
' steering command.
'
' Other Inputs:
' PD2 - MISO from V2X Serial Data Out
' PE0 - from V2X CALI 'not currently used by software
' PE1 - from V2X EOC
' Other Outputs:
' PD3 - MOSI to V2X P/C
' PD4 - SCK to V2X SCLK
' PD5 - SS to V2X SS
' Vector 2X is operated in polled slave mode (HC11 is master),
' low res, SPIE=0,SPE=1,DWOM=0,CPOL=CPHA=1,E/2,binary output
' Note: Version 1.04 of Vector 2X notes highly desirable
'
' procedure:
' >sbasic sigproc.bas /s87FF /v9000 /cA000 > sigproc.asc
' puts stack at 87FF, variables at 9000, code at A000. If no errs in sigproc.asc
' >asmhc11 sigproc.asc, result is sigproc.s19
' if no errs during compile, reset Tcomp in bootstrap (both buttons) mode, then
' >pcbug11 -E
' ms $103c,$e5 <- sets HPRIO to E5
' eeprom $103c $103e
' loads sigproc <- loads sigproc.s19 starting at $A000
' term <- goes into teminal mode to display print results
' hit reset (only) on Tcomp to begin execution at $A000
'
' empty for-next loop: 71 counts thru = 1 msec delay (28 ticks/loop)
' 14 microsec/loop
' for i=-30000 to 30000 next <- takes 1 sec
'
' absolute addresses appended with 'a'
'
DECLARE BAUDa
DECLARE SCCR2a
DECLARE HPRIOa
DECLARE OC1Ma
DECLARE OC1Da
DECLARE TCTL1a
DECLARE TCTL2a
DECLARE TMSK1a
DECLARE TFLG1a
DECLARE PACTLa
DECLARE TOC1a
DECLARE TOC2a
DECLARE TOC3a
DECLARE TOC4a
DECLARE TIC1a
DECLARE TIC2a
DECLARE TIC3a
DECLARE TIC4a
DECLARE SPCRa
DECLARE SPSRa
DECLARE SPDRa
DECLARE PORTDa
DECLARE PORTEa
DECLARE DDRDa
DECLARE FALSE
DECLARE TRUE
DECLARE MAYBE
DECLARE byte1
DECLARE byte2
DECLARE tmpcmdout1
DECLARE tmpcmdout2
DECLARE tmpcmdout3
DECLARE tmpcmdout4
DECLARE i
DECLARE j
DECLARE k
DECLARE Kst
DECLARE heading
DECLARE maintain_heading
DECLARE no_steering_cmd
DECLARE heading_error
DECLARE heading_correction
DECLARE target_heading
DECLARE first_pass
DECLARE highpoint1
DECLARE highpoint2
DECLARE highpoint3
DECLARE highpoint4
DECLARE midpoint1
DECLARE midpoint2
DECLARE midpoint3
DECLARE midpoint4
DECLARE lowpoint1
DECLARE lowpoint2
DECLARE lowpoint3
DECLARE lowpoint4
DECLARE deadlow1
DECLARE deadhigh1
DECLARE range1
DECLARE range2
DECLARE range3
DECLARE range4
DECLARE band1
DECLARE band2
DECLARE band3
DECLARE band4
DECLARE cmd1
DECLARE cmd2
DECLARE cmd3
DECLARE cmd4
DECLARE pulse1_length
DECLARE pulse2_length
DECLARE pulse3_length
DECLARE pulse4_length
DECLARE hold_this1
DECLARE hold_this2
DECLARE hold_this3
DECLARE hold_this4
DECLARE pw1keep(10)
DECLARE pw2keep(10)
DECLARE pw3keep(10)
DECLARE pw4keep(10)
DECLARE sum1
DECLARE sum2
DECLARE sum3
DECLARE sum4
DECLARE smooth1
DECLARE smooth2
DECLARE smooth3
DECLARE smooth4
DECLARE span
DECLARE x
DECLARE y
DECLARE z
MAIN: 'begin execution at reset here
'
'initialize variables
'
interrupts off
span=5 'max is 10
for i=0 to span-1
pw1keep(i)=0
pw2keep(i)=0
pw3keep(i)=0
pw4keep(i)=0
next
sum1=0
sum2=0
sum3=0
sum4=0
pulse4_length=0
hold_this4=0
pulse3_length=0
hold_this3=0
pulse2_length=0
hold_this2=0
pulse1_length=0
hold_this1=0
cmd4=0
cmd3=0
cmd2=0
cmd1=0
band1=100
band2=0
band3=0
band4=0
OC1Ma = $100C
OC1Da = $100D
HPRIOa= $103C
TCTL1a= $1020
TCTL2a= $1021
TMSK1a= $1022
TFLG1a= $1023
PACTLa= $1026
TOC1a = $1016
TOC2a = $1018
TOC3a = $101A
TOC4a = $101C
TIC1a = $1010
TIC2a = $1012
TIC3a = $1014
TIC4a = $101E
BAUDa = $102b
SCCR2a= $102d
SPCRa = $1028
SPSRa = $1029
SPDRa = $102A
PORTDa= $1008
PORTEa= $100A
DDRDa = $1009
FALSE=0
TRUE=1
MAYBE=2
'
'below numbers measured from R/C transmitter
'
highpoint1=4055
highpoint2=3959
highpoint3=3757
highpoint4=3985
midpoint1=3093
midpoint2=3062
midpoint3=3032
midpoint4=3062
lowpoint1=2033
lowpoint2=2039
lowpoint3=2310
lowpoint4=2075
range1=highpoint1-lowpoint1
range2=highpoint2-lowpoint2
range3=highpoint3-lowpoint3
range4=highpoint4-lowpoint4
deadlow1=midpoint1-band1
deadhigh1=midpoint1+band1
Kst=20 'proportional correction gain
maintain_heading=TRUE
no_steering_cmd=TRUE
first_pass=TRUE
heading_error=0
heading_correction=0
target_heading=0
'
' set up IC and OC configurations
'
poke TOC1a,0
poke TOC2a,midpoint2
poke TOC3a,midpoint3
poke TOC4a,midpoint4
'
pokeb HPRIOa, %00101111 ' expanded mode, IC4 top priority
pokeb PACTLa, %10000100 ' set DDRA7, I4/O5
pokeb TCTL2a, %01010101 ' set to rising edges for IC4,1,2,3 (0:1)
pokeb OC1Ma, %11110000 ' set OC1 to control OC1,2,3,4
pokeb OC1Da, %11110000 ' set OC1,2,3,4 to go high at OC1 trigger
pokeb TCTL1a, %10101000 ' set OC2,3,4 low at TOC2,3,4 trip (1:0)
pokeb TMSK1a, %11111111 ' enable OC1,2,3,4 and IC4,1,2,3
'
cmd4=midpoint4
cmd3=midpoint3
cmd2=midpoint2
cmd1=midpoint1
'
INTERRUPTS ON ' enable all interrupts
'
' set up to allow print to terminal
'
pokeb BAUDa, $30 '9600 baud
pokeb SCCR2a,$0c 'enable SCI rcvr & xmtr
print "Hello, world!"
'
' set up SPI
'
pokeb DDRDa,%00111010 'outp -,-,PD5/SS,PD4/SCK,PD3/MOSI,!PD2/MISO,PD1/TxD
pokeb PORTDa,peekb(PORTDa) OR %00101000 'raise PD5/SS & PD3/POLL lines
pokeb PORTDa,peekb(PORTDa) AND %11111011 'clear PD2
pokeb PORTEa,0 'just to make sure pull-up resistor doesn't register
print "hit the reset button on the V2X within 3 seconds"
for i=-30000 to 30000 next
for i=-30000 to 30000 next
for i=-30000 to 30000 next
'*************************************************
'
' Main loop
'
'*************************************************
do ' main loop forever
'
' acquire heading data from Vector 2X
'
' send out a 10 msec low pulse from PD3/MOSI to P/C
'
pokeb PORTDa,peekb(PORTDa) AND %11110111 'clr PD3 P/C
waitwhile PORTEa,$02 'wait for EOC to go low
for i=0 to 710 next 'wait >10 msec
pokeb PORTDa,peekb(PORTDa) OR %00001000 'set PD3 P/C
waituntil PORTEa,$02 'wait for EOC to go high again
for i=1 to 710 next ' wait 10 msec
pokeb PORTDa,peekb(PORTDa) AND %11011111 'lower PD5/SS line
for i=0 to 710 next
pokeb SPCRa,%01011100 'enable SPI*********************
' !SPIE,SPE,!DWOM,MSTR,CPOL,CPHA,E/2
pokeb SPDRa,$FF 'start receiving while transmitting 1's
'transmitting 1's keeps P/C line high
'
' transmission now going...
'
waituntil SPSRa,%10000000 'wait till SPIF set (done)
byte1=peekb(SPDRa) 'get first byte and clear SPIF
pokeb SPDRa,$FF 'write to SPDR to initiate SPI transfer
waituntil SPSRa,%10000000 'wait till SPIF set
byte2=peekb(SPDRa) 'get 2nd byte, clr SPIF
pokeb SPCRa,%00011100 'disable SPI*********************
' !SPIE,!SPE,!DWOM,MSTR,CPOL,CPHA,E/2
'
' send out 1 more clock tic to keep V2X happy, a desperate
' last-ditch effort to get this thing to work, which was
' uncharacteristically successful, so in it stays
'
pokeb PORTDa,peekb(PORTDa) AND %11101111 ' set sclk low
pokeb PORTDa,peekb(PORTDa) OR %00010000 ' set sclk high
pokeb PORTDa, peekb(PORTDa) OR %00100000 ' set PD5/SS line
pokeb addr(heading),byte1 'stuff bytes into variable
pokeb addr(heading)+1,byte2
if (peekb(SPSRa) AND %11010000)<>0 print "SPI err" endif
if first_pass=TRUE 'load in initial target_heading, fire up RC
print "turn on the RC transmitter within 3 seconds"
for i=-30000 to 30000 next
for i=-30000 to 30000 next
for i=-30000 to 30000 next
first_pass=FALSE
target_heading=heading
endif
'
' no_steering_cmd is maintained by the IC1 ISR
'
if no_steering_cmd=FALSE 'a steering command is coming in
select maintain_heading
case TRUE 'may be false alarm, keep correcting
maintain_heading=MAYBE
heading_error=target_heading-heading
heading_correction=-Kst*heading_error
endcase
case MAYBE 'twice now, keep correcting
maintain_heading=FALSE
heading_error=target_heading-heading
heading_correction=-Kst*heading_error
endcase
case FALSE ' that's it, update target_heading
target_heading=heading
heading_error=0
heading_correction=0
endcase
endselect
else
maintain_heading=TRUE 'include error corrections in steering
heading_error=target_heading-heading
heading_correction=-Kst*heading_error
endif
' print cmd1,"\t",cmd2,"\t",cmd3,"\t",cmd4,"\t",heading_correction
' print "mh= ",maintain_heading," nsc= ", no_steering_cmd
print heading,target_heading,heading_error,heading_correction,cmd1
' print pulse1_length,pulse2_length,pulse3_length,pulse4_length
loop ' end of main loop
'
' can't reach below code, so sue me
'
pokeb TMSK1a, 0 ' disable OC1,2,3,4 and IC4,1,2,3
poke TOC4a, 0
poke TOC3a, 0
poke TOC2a, 0
poke TOC1a, 0
print "done\n"
return
END 'main
'
'**********************************************************
'
' end of main routine
'
'**********************************************************
'
' Now load in OC and IC interrupt routines
' Note: much processing has been offloaded into these routines
' so care must be taken when RC is fired up to avoid swamping
' the program with IRQ's during intialization
'
INTERRUPT $FFE8 ' OC1 int vector
' OC1 serves two functions: on the first pass through, when TCNT overflows,
' OC1 sets OC1,2,3,4 high. It then disconnects itself from OC2,3,4, and sets
' only OC1 to go low at TOC1 <- cmd1. On the second pass through, TOC1 is set
' back to zero and OC1,2,3,4 are all reconnected to go high at TOC1=0 trigger
if peek(TOC1a) = 0 'pulse has just begun on OC1,2,3,4
' TOC1 was zero, first pass OC1-4 now all set high
' now set TOC1 to trigger again alone, in CMD1 time
poke TOC1a,cmd1
' set OC1 to go low at next TOC1=cmd1 trigger
pokeb OC1Da,peekb(OC1Da) AND %01111111
' disconnect OC2,3,4 from the next OC1
pokeb OC1Ma,peekb(OC1Ma) AND %10001111
else
' pulse done
' set TOC1 to zero, ready to start cycle again
' reconnect other OC's for beginning of pulse
poke TOC1a,0
' set OC1 to go high at next TOC1
pokeb OC1Da,peekb(OC1Da) OR %10000000
' reconnect OC,2,3,4 for the next OC1 start
pokeb OC1Ma,peekb(OC1Ma) OR %11110000
endif
pokeb TFLG1a, %10000000 ' write a 1 to set irq done flag
return ' writing a 0 to a set flag does not disturb the flag
end
INTERRUPT $FFE6 ' OC2 int vector
pokeb TFLG1a, %01000000
return
end
INTERRUPT $FFE4 ' OC3 int vector
pokeb TFLG1a, %00100000
return
end
INTERRUPT $FFE2 ' OC4 int vector
pokeb TFLG1a, %00010000
return
end
INTERRUPT $FFE0 ' IC4 int vector 'go (drive) command intercept
'set to capture on rising edge first, once tripped, set to capture falling edge
if (peekb(TCTL2a) AND %01000000)<>0 'was set to capture rising edge
hold_this4 = peek(TIC4a) ' time of rising edge
pokeb TCTL2a,peekb(TCTL2a) AND %10111111
pokeb TCTL2a,peekb(TCTL2a) OR %10000000 ' set to 1:0 falling
pokeb TFLG1a,%00001000 'write a 1 to clr irq flg
return
else ' was set to capture falling edge
pulse4_length=peek(TIC4a)-hold_this4 'length=falling-rising
pokeb TCTL2a,peekb(TCTL2a) AND %01111111
pokeb TCTL2a,peekb(TCTL2a) OR %01000000 ' set to 0:1 rising
endif
'
' below code only executed if a falling edge was captured
'
tmpcmdout4=pulse4_length 'go command
'
' limit command
'
if tmpcmdout4< lowpoint4 tmpcmdout4= lowpoint4
elseif tmpcmdout4>highpoint4 tmpcmdout4=highpoint4
endif
'
' smooth command over (span) readings (this code is rough,
' could use some efficiency improvement)
'
sum4=0 'compute sum over span
for i=0 to span-2
pw4keep(i)=pw4keep(i+1) ' move window forward
sum4=sum4+pw4keep(i)
next
pw4keep(span-1)=tmpcmdout4
smooth4=sum4/span ' window average
cmd4=smooth4
poke TOC4a, cmd4 'set TOC4 to output command as is
pokeb TFLG1a,%00001000 'write a 1 to clr irq flg
return
end
INTERRUPT $FFEE ' IC1 int vector 'steering command intercept
if (peekb(TCTL2a) AND %00010000)<>0
hold_this1 = peek(TIC1a)
pokeb TCTL2a,peekb(TCTL2a) AND %11101111
pokeb TCTL2a,peekb(TCTL2a) OR %00100000
pokeb TFLG1a, %00000100
return
else
pulse1_length=peek(TIC1a)-hold_this1
pokeb TCTL2a,peekb(TCTL2a) AND %11011111
pokeb TCTL2a,peekb(TCTL2a) OR %00010000
endif
tmpcmdout1=pulse1_length+heading_correction
if tmpcmdout1< lowpoint1 tmpcmdout1= lowpoint1
elseif tmpcmdout1>highpoint1 tmpcmdout1=highpoint1
endif
sum1=0
for i=0 to span-2
pw1keep(i)=pw1keep(i+1) ' move window forward
sum1=sum1+pw1keep(i)
next
sum1=sum1+tmpcmdout1
pw1keep(span-1)=tmpcmdout1
smooth1=sum1/span ' window average
cmd1=smooth1 'set cmd1, but let OC1 load TOC1
'
' if command lies in deadband, set no_steering_cmd=TRUE
'
no_steering_cmd=FALSE
if tmpcmdout1>deadlow1
if tmpcmdout1<deadhigh1
no_steering_cmd=TRUE
endif
endif
pokeb TFLG1a, %00000100
return
end
INTERRUPT $FFEC ' IC2 int vector 'pan command intercept
if (peekb(TCTL2a) AND %00000100)<>0
hold_this2 = peek(TIC2a)
pokeb TCTL2a,peekb(TCTL2a) AND %11111011
pokeb TCTL2a,peekb(TCTL2a) OR %00001000
pokeb TFLG1a,%00000010
return
else
pulse2_length=peek(TIC2a)-hold_this2
pokeb TCTL2a,peekb(TCTL2a) AND %11110111
pokeb TCTL2a,peekb(TCTL2a) OR %00000100
endif
tmpcmdout2=pulse2_length 'pan
if tmpcmdout2< lowpoint2 tmpcmdout2= lowpoint2
elseif tmpcmdout2>highpoint2 tmpcmdout2=highpoint2
endif
sum2=0
for i=0 to span-2
pw2keep(i)=pw2keep(i+1) ' move window forward
sum2=sum2+pw2keep(i)
next
pw2keep(span-1)=tmpcmdout2
smooth2=sum2/span ' window average
cmd2=smooth2
poke TOC2a, cmd2
pokeb TFLG1a,%00000010
return
end
INTERRUPT $FFEA ' IC3 int vector 'tilt command intercept
if (peekb(TCTL2a) AND %00000001)<>0
hold_this3 = peek(TIC3a)
pokeb TCTL2a,peekb(TCTL2a) AND %11111110
pokeb TCTL2a,peekb(TCTL2a) OR %00000010
pokeb TFLG1a, %00000001
return
else
pulse3_length=peek(TIC3a)-hold_this3
pokeb TCTL2a,peekb(TCTL2a) AND %11111101
pokeb TCTL2a,peekb(TCTL2a) OR %00000001
endif
tmpcmdout3=pulse3_length 'tilt
if tmpcmdout3< lowpoint3 tmpcmdout3= lowpoint3
elseif tmpcmdout3>highpoint3 tmpcmdout3=highpoint3
endif
sum3=0
for i=0 to span-2
pw3keep(i)=pw3keep(i+1) ' move window forward
sum3=sum3+pw3keep(i)
next
pw3keep(span-1)=tmpcmdout3
smooth3=sum3/span ' window average
cmd3=smooth3
poke TOC3a, cmd3
pokeb TFLG1a, %00000001
return
end
* Michael L. Ross/C33 | Lockheed Engineering & Sciences Co.****
* Robotics Department | 2400 Nasa Rd. 1, Houston, TX 77058*****
*(713)333-7094 voice,(713)333-6071 fax*#include <disclaimer.h>*
* Finger me for my public key. mross@outland.jsc.nasa.gov ****
* If you weren't a socialist in your twenties, you had no heart.
* If you weren't a conservative by your thirties, you have no brain.
|
|
1 Message in This Thread:
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
