UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools


projects:ukhas_glider_project:slave
$regfile = "m168def.dat"
$crystal = 16000000
$baud = 16000
$hwstack = 64
$swstack = 64
$framesize = 64
CONST Servogain = 10
CONST Pterm = -0.25                                         'all -ive as a +ive pulse lenght causes +ive turn
CONST Iterm = -0.0003
CONST Dterm = -0.35
CONST Kalmangpsweight = 0.5
CONST Kalmangyroweight = 0.5
CONST Propogation = 0.98 *(1 -kalmangyroweight)
CONST Servocontrolinput = 0.0015 *(1 -kalmangyroweight)     'need to check these with simcode
CONST Center = 6200                                         'needs to be checked with servo
CONST Conversionfactor = 16                                 'pwm timer counter is 16 times slower than sw pwm
CONST Lowerpwm = Center - 300
CONST Upperpwm = Center + 300
Baud = 125000
Config Portd.4 = OUTPUT
Config Portd.2 = OUTPUT
Config Portd.5 = OUTPUT
Config Portb.1 = OUTPUT
Config Portd.3 = INPUT
Config Portd.6 = INPUT
Config Portd.7 = INPUT
Config Portb.0 = INPUT
Config Portc.4 = INPUT
Config Portc.5 = OUTPUT
Config Portc.2 = OUTPUT
Config Portc.3 = OUTPUT
Config Watchdog = 1024
'Config Spi = Soft , Din = Pinc.4 , Dout = Portc.5 , Ss = None , Clock = Portc.2
'Spiinit
Config Timer1 = TIMER , Prescale = 64
Config Timer0 = TIMER , Prescale = 1024
DIM Rate AS INTEGER , Temperature AS INTEGER , Lsb AS Byte , Msb AS Byte       'melexis
DIM Rateread AS Byte , Tempread AS Byte , Readmelexis AS Byte , Mask AS Word , Maskword AS Word , Servoenable AS Byte
DIM Firstupdate AS Byte , Servoexcitecount AS Byte          'rest
DIM S AS SINGLE , R AS SINGLE , Gpfloatone AS SINGLE , Gpfloattwo AS SINGLE , Oldheading AS SINGLE , Realrate AS SINGLE , Integral AS SINGLE , Filteredrate AS SINGLE , Servo AS SINGLE
DIM Gpbyteone AS Byte , Pulses AS Byte , ERROR AS Byte , Enablegroundcontrol AS Byte
DIM U AS INTEGER , Gpintegerone AS INTEGER , Targetheading AS SINGLE , Gpsheading AS SINGLE , Heading AS SINGLE
DIM Gpwordone AS Word , Pulselenght AS Word
Rateread = &B10010100
Tempread = &B10011100
Readmelexis = &B10000000
Maskword = &B0000111111111111
Pcmsk2 = &B01000000                                         'pcint22 enabled
Pcmsk0 = &B00000001                                         'pcint0 enabled
Groundcontrol ALIAS Portd.4
Rts ALIAS Pind.3
Cts ALIAS Portd.2
Servoone ALIAS Portd.5
Servotwo ALIAS Portb.1
Pulsesdetect ALIAS Pind.6
Inputone ALIAS Pinb.0
Inputtwo ALIAS Pind.7
Slave ALIAS Portc.3
Din ALIAS Pinc.4
Dout ALIAS Portc.5
Clock ALIAS Portc.2
Slave = 1
'Declare Sub Ground()
DECLARE SUB Talktomelexis()
'Declare Sub Ascent()
ON Pcint0 Edge
ON Pcint2 Copy
Set Pcifr.pcif2
Set Pcifr.pcif0
Enable Pcint2
Enable Pcint0
Groundcontrol = 0
Servotwo = 1
Servoone = 1
S = 0
Integral = 0
FOR Gpbyteone = 0 TO 200                                    'to let us check servo alignment
 Waitms 15
 Pulseout , Portd , 5 , Center
NEXT
Start Timer1
Start Timer0
Enable Interrupts
Start Watchdog
 
DO
 RESET Watchdog
 Waitms 15
 IF Pulselenght > 200 AND Pulselenght < 600 AND Timer1 < 20000 THEN       'a valid and recent pulse
  Incr Pulses
 ELSE
  Pulses = 0
  Groundcontrol = 0
 END IF
 IF Pulses > 10 THEN
  Groundcontrol = 1                                         'we enable servo relaying
  Pulses = 10
 END IF
 IF Groundcontrol = 0 THEN
  Servoone = 1
  Pulseout , Portd , 5 , Center                             'keep servo centered
 END IF
 CALL Talktomelexis()                                       'check gyro
 IF Rts = 1 THEN                                            'check for comms
  Disable Interrupts                                        'disable interrupts with first message
  Cts = 1
  Inputbin Gpbyteone
  Cts = 0
  IF Gpbyteone = 1 THEN                                     'if we get a descent command
    Firstupdate = 2                                         'means we treat the first gps update correctly
    Waitus 300
    Printbin Rate ; Temperature ; ERROR ; Heading ; Filteredrate ; Pulselenght ; Pulses       'removes possible cause of a hang
    GOTO Descent
  END IF
  Waitus 300
  Printbin Rate ; Temperature ; ERROR
  Servoone = 1
  Incr Servoexcitecount
  IF Servoexcitecount > 10 THEN
   Servoexcitecount = 0
   FOR Gpbyteone = 0 TO 10
    Pulseout , Portd , 5 , Lowerpwm                         'wiggle servo
    Waitms 15
   NEXT
   FOR Gpbyteone = 0 TO 10
    Pulseout , Portd , 5 , Upperpwm
    Waitms 15
   NEXT
  END IF
 END IF
LOOP
 
Descent:
DO
 RESET Watchdog
 WHILE Timer0 < 250                                         'wait for 16ms to elapse
 WEND
 RESET Timer0
 IF Pulselenght > 200 AND Pulselenght < 600 AND Timer1 < 20000 THEN       'a valid and recent pulse
  Incr Pulses
 ELSE
  Pulses = 0
  Groundcontrol = 0
 END IF
 IF Pulses > 10 THEN
  Groundcontrol = 1                                         'we enable servo relaying
  Pulses = 10
 END IF
IF Rts = 1 THEN                                             'check for comms
 Cts = 1
 Inputbin Gpbyteone , Gpsheading , Targetheading , Servoenable , Enablegroundcontrol
 Cts = 0
 IF Firstupdate = 0 OR Firstupdate = 1 THEN                 'ie we had another message not long ago
  Gpfloatone = Gpsheading - Oldheading                      'so update the heading estimate
  IF Gpfloatone < -180 THEN                                 'otherwise we dont bother as we dont know what was happening
   Gpfloatone = Gpfloatone + 360                            'one second ago
  END IF
  IF Gpfloatone > 180 THEN
   Gpfloatone = Gpfloatone - 360                            'get it in the right range
  END IF
  IF Firstupdate = 0 THEN
   Gpfloatone = Kalmangpsweight * Gpfloatone                'if firstupdate=1, gpsgain=1
  ELSE                                                      'if firstupdate=1, set to zero
   Firstupdate = 0
  END IF
  Heading = Heading + Gpfloatone
  IF Heading > 180 THEN                                     'get heading in correct range
   Heading = Heading - 360
  END IF
  IF Heading < -180 THEN
   Heading = Heading + 360
  END IF
 ELSE                                                       'ie if firstupdate=2
  Targetheading = 0                                         'we set targetheading to zero for first update, as heading is relative
  Firstupdate = 1                                           'until we get more recent data, due to gps lag
 END IF
 Oldheading = Heading                                       ' always store the past heading
 IF Enablegroundcontrol = 1 THEN                            'enable ground control
  Enable Interrupts
 ELSE
  Disable Interrupts
 END IF
 Waitus 250
 Printbin Rate ; Temperature ; ERROR ; Heading ; Filteredrate ; Pulselenght ; Pulses
END IF
CALL Talktomelexis()
Gpintegerone = Rate
Gpintegerone = Gpintegerone - 1009                          'center position
Gpfloatone = Gpintegerone
Gpfloatone = Gpfloatone / 6.8265
Realrate = Gpfloatone                                       'now  in degrees per second
Gpfloatone = Gpfloatone * Kalmangyroweight                  'gyro term
Filteredrate = Propogation * Filteredrate                   'propogation term
Filteredrate = Filteredrate + Gpfloatone                    'add them
Gpfloatone = Servo * Servocontrolinput                      'control term - need to use rx pwm if under ground control
Filteredrate = Filteredrate + Gpfloatone                    'add them
Gpfloatone = Filteredrate / 62.5                            'numerical integration over timestep
Heading = Heading + Gpfloatone
IF Heading > 180 THEN                                       'get heading in correct range
 Heading = Heading - 360
END IF
IF Heading < -180 THEN
 Heading = Heading + 360
END IF
Gpfloatone = Heading - Targetheading                        'heading offset
IF Gpfloatone < -180 THEN                                   'get it in the right range
 Gpfloatone = Gpfloatone + 360
END IF
IF Gpfloatone > 180 THEN
 Gpfloatone = Gpfloatone - 360
END IF
Integral = Integral + Gpfloatone                            'i increment                                                     'pid
Gpfloattwo = Gpfloatone * Pterm                             'P
Gpfloatone = Dterm * Filteredrate                           'D
Gpfloatone = Gpfloatone + Gpfloattwo                        'add d and p terms
IF Gpfloatone > 30 THEN Gpfloatone = 30                     'servo limits on d and p
IF Gpfloatone < -30 THEN Gpfloatone = -30
Gpfloattwo = Integral * Iterm                               'calculate I
IF Gpfloattwo > 25 THEN Integral = 25 / Iterm               'wind up prevension
IF Gpfloattwo < -25 THEN Integral = -25 / Iterm
Gpfloatone = Gpfloatone + Gpfloattwo                        'add I term
Gpfloatone = Servogain * Gpfloatone                         'servo dependant variable
Servo = Gpfloatone                                          'store servo for control input
Gpintegerone = Gpfloatone
IF Groundcontrol = 1 THEN
 Gpfloatone = Pulselenght * Conversionfactor
 Servo = Gpfloatone - Center                                'uses rx pwm if under ground control
END IF
Gpintegerone = Gpintegerone + Center                        'add on pwm center value
IF Groundcontrol = 0 AND Servoenable = 1 THEN
 Servoone = 1                                               'set pwm high
 Pulseout , Portd , 5 , Gpintegerone                        'only if not under ground control
END IF
LOOP
 
Edge:
IF Groundcontrol = 1 THEN Servoone = NOT Inputone           'copy
IF Inputone = 0 THEN
 Pulselenght = Timer1                                       'read pwm
END IF
Timer1 = 0
RETURN
 
Copy:
IF Groundcontrol = 1 THEN Servotwo = NOT Inputtwo           'copy
RETURN
 
SUB Talktomelexis()
 Clock = 0
 Dout = 0
 Slave = 0
 ERROR = 0
 Waitus 1
 FOR Gpbyteone = 7 TO 0 STEP -1                             'spi out
  Dout = Rateread.gpbyteone
  Waitus 1
  Clock = 1
  Waitus 1
  Clock = 0
 NEXT
 FOR Gpbyteone = 15 TO 0 STEP -1                            'extra clock cycles is in datasheet
  Waitus 1
  Clock = 1
  Waitus 1
  Clock = 0
 NEXT
 Waitus 1
 Slave = 1
 Waitus 300
 Slave = 0
 FOR Gpbyteone = 7 TO 0 STEP -1                             'spi out
  Dout = Readmelexis.gpbyteone
  Waitus 1
  Clock = 1
  Waitus 1
  Clock = 0
 NEXT
 FOR Gpbyteone = 7 TO 0 STEP -1                             'data in
  Waitus 1
  Clock = 1
  Msb.gpbyteone = Din
  Waitus 1
  Clock = 0
 NEXT
 FOR Gpbyteone = 7 TO 0 STEP -1                             'another byte in
  Waitus 1
  Clock = 1
  Lsb.gpbyteone = Din
  Waitus 1
  Clock = 0
 NEXT
 Waitus 1
 Slave = 1
 Rate = Makeint(lsb , Msb)                                  'create integer
 Rate = Rate AND Maskword                                   'mask it
 Shift Rate , Right , 1                                     'shift it
 IF Msb.6 = 1 THEN ERROR = 1                                'check selftest feature
 Slave = 0
 Waitus 1
 FOR Gpbyteone = 7 TO 0 STEP -1                             'do the same sort of thing for temp
  Dout = Tempread.gpbyteone
  Waitus 1
  Clock = 1
  Waitus 1
  Clock = 0
 NEXT
 FOR Gpbyteone = 15 TO 0 STEP -1
  Waitus 1
  Clock = 1
  Waitus 1
  Clock = 0
 NEXT
 Waitus 1
 Slave = 1
 Waitus 300
 Slave = 0
 FOR Gpbyteone = 7 TO 0 STEP -1
  Dout = Readmelexis.gpbyteone
  Waitus 1
  Clock = 1
  Waitus 1
  Clock = 0
 NEXT
 FOR Gpbyteone = 7 TO 0 STEP -1
  Waitus 1
  Clock = 1
  Msb.gpbyteone = Din
  Waitus 1
  Clock = 0
 NEXT
 FOR Gpbyteone = 7 TO 0 STEP -1
  Waitus 1
  Clock = 1
  Lsb.gpbyteone = Din
  Waitus 1
  Clock = 0
 NEXT
 Waitus 1
 Slave = 1
 Temperature = Makeint(lsb , Msb)
 IF Msb.6 = 1 THEN ERROR = 1
 Temperature = Temperature AND Maskword
 Shift Temperature , Right , 1
END SUB
projects/ukhas_glider_project/slave.txt ยท Last modified: 2008/07/19 23:33 (external edit)