Code:
Data @0,$0D, $0A, "P4 Servo ver 1.05 BDT 21/03/2006",$0D, $0A
data @40, 0, 0 'error counter
'****************************************************************
'* Name : P4_Servo_V105.BAS *
'* Author : Brian Taylor *
'* Notice : Copyright (c) 2006 Brian Taylor *
'* : All Rights Reserved *
'* Date : 21/03/2006 *
'* Version : 1.05 *
'* Notes : *
'* : *
'****************************************************************
' This is P4 - 16F88 - the servo driver
' test code to debug the hardware
'
'Background.
' A 'standard' JR, Futaba, HS, etc RC system uses on a nominal 50 Hz
' refresh rate to the servos. This gives a nominal 20 mSec frame time.
' Servos centre at 1.5 mSec and range from 1 to 2 mSec at the end stops.
' Bitter experience shows digital servos burn out outside 1-2 mS values.
' 8 channels all at maximum, highly unlikely, takes 8 x 2 mSec leaving
' 4 mSec for sync, etc. 8 channels all at minimum width, also highly
' unlikely, takes 8 mS leaving 12 mSec for sync, processing, etc.
' An 'average' frame has 8 channels at 1.5 mSec leaving 8 mSec for playtime.
' This code gets a 'fast packet' from the leveller PIC in about 2.1 mSec
' and drives all 8 servos regardless of whether they exist in hardware.
'
DEFINE LOADER_USED 1
DEFINE OSC 20
define no_clrwdt 1
define loader_used 1
' Set Debugin pin port
DEFINE DEBUGIN_REG PORTB
DEFINE DEBUG_BAUD 115200 '76800
' Set Debugin pin bit
DEFINE DEBUGIN_BIT 7
' Set Debugin mode: 0 = true, 1 = inverted
DEFINE DEBUGIN_MODE 1
'@ Device pic16F88, HS_OSC, BOD_OFF, PWRT_ON, WDT_ON, PROTECT_OFF
'************************** Hardware assignment *************************
'Port A all digital
Svo8 var porta.0 'p 17 digital drive to servo 8 digital out
Svo7 var porta.1 'p 18 servo 7 digital out
Svo6 var porta.2 'p 1 digital drive to servo 6 digital out
Svo5 var porta.3 'p 2 digital drive to servo 5 digital out
Svo4 var porta.4 'p 3 digital drive to servo 4 digital out
TRISA = %11100000
ANSEL = %00000000
CMCON = %00000111
'Port B
Svo3 var portb.0 'pin 6 servo 3 drive out
Svo2 var portb.1 'pin 7 servo 2 out
RxD var portb.2 'pin 8 BootLoader input in
Svo1 var portb.3 'pin 9 Servo 1 drive out
P4LED var portb.4 'pin 10 Diagnostic LED out
TxD var portb.5 'pin 11 BootLoader output - diagnostic out out
P3_P4 var portb.6 'p 12 Data in from P3 in
P2_P4 var portb.7 'p 13 Servo packet data from P2 in
TRISB = %11000100
'****************************** Software assignment **************************
A var byte
B var byte
C var byte
W var word
InCtr var byte
Ch var byte[8] ' the current channel data
LastCh var byte[8] ' last clean channel data
FailCh var byte[8] ' failsafe values invoked after N bad packets
Delta var byte ' maximum allowable servo step
CheckSum var byte
ErrorCtr var word ' counts checksum errors in servo data from P2
StepLimit con 8 ' max allowed servo change per frame
' damps out violent spikes
'--------------------------------- Initialise ------------------------
Initialise:
ShowRevision:
serout txd, 2, [$0D, $0A]
for a = 0 to 79
read a,b
serout txd, 2, [b]
clearwdt
next a
serout txd, 2, [$0D, $0A]
' WDTCON = %00001001 ' invoke s/w WDT, 58 mSec timeout, 110 mSec recovery
WDTCON = %00001010 ' longer WDT activate period
OPTION_REG = %10001010 ' No wpu, prescaler assigned to WDT & /4
' OPTION_REG xxxx00 gives 15 mS, 10 gives 64 mS
INTCON = %00000000 ' disable all interrupts
ANSEL = %00000000 ' all digital
CMCON = %00000111 ' Comparators OFF to allow inputs on pins
ADCON0 = %00000000
ADCON1 = %00000000
SetInitialServoPosition:
Ch[0] = 0
Ch[1] = 125
Ch[2] = 125
Ch[3] = 125
Ch[4] = 125
Ch[5] = 125
Ch[6] = 125
Ch[7] = 125
high p4led ' fires if WDT is invoked during preflight ground check.
Start:
GetServoData:
debugin 90, failsafe, [a, ch[0], ch[1], ch[2], ch[3], ch[4], ch[5], _
ch[6], ch[7], checksum]
'A = throw away sync byte - often corrupted
c = 0 ' checksum comparison
for b = 0 to 7 ' build local checksum
c = c + ch[b]
next b
if checksum = c then
goto goodpacket
endif
Failsafe: ' Use last clean packet if internal processor to processor
' error occurs
high p4led
for a = 0 to 7
ch[a] = lastch[a]
next a
GoodPacket:
DriveServos:
' PULSOUT gives 2 uS steps at 20 MHz.
' The servos need 1000 to 2000 uS at 50 Hz
' MUST MUST MUST define initial states as Pulsout is a toggle.
svo1 = 0
svo2 = 0
svo3 = 0
svo4 = 0
svo5 = 0
svo6 = 0
svo7 = 0
svo8 = 0
clearwdt
w = 500 + 2*ch[0] ' W is in 2 uS steps = 1000 to 2000 uS
pulsout svo1, w
w = 500 + 2*ch[1]
pulsout svo2, w
w = 500 + 2*ch[2]
pulsout svo3, w
w = 500 + 2*ch[3]
pulsout svo4, w
w = 500 + 2*ch[4]
pulsout svo5, w
w = 500 + 2*ch[5]
pulsout svo6, w
w = 500 + 2*ch[6]
pulsout svo7, w
w = 500 + 2*ch[7]
pulsout svo8, w
SaveLastValues:
for a = 0 to 7
lastch[a] = ch[a]
next a
low p4led
clearwdt
goto start 'getservodata
HTH
Bookmarks