projects:ukhas_glider_project:slave
This is an old revision of the document!
$regfile = "m168def.dat" $crystal = 16000000 $baud = 16000 $hwstack = 64 $swstack = 64 $framesize = 64 CONST Servogain = 10 CONST Pterm = 1 'needs to be -1 here? as gyro will be chip facing up, not down CONST Iterm = 0.001 CONST Dterm = 0 CONST Kalmangpsweight = 1 CONST kalmangyroweight=0.05 CONST propogation=0.98*(1-kalmangyroweight) CONST servogain=-0.007*(1-kalmangyroweight) 'need to check these with simcode CONST Center = 6000 'needs to be checked with servo 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 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 T AS STRING * 30 'rest DIM Contents(5) AS STRING * 20 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 , Timeout AS Byte , ERROR 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 Pcint2 Edge ON Pcint0 Copy Set Pcifr.pcif2 Set Pcifr.pcif0 Enable Pcint2 Enable Pcint0 Servotwo = 1 Servoone = 1 S = 0 Start Timer1 Start Timer0 Enable Interrupts WAIT 2 DO Incr Timeout IF Timeout > 250 THEN Timeout = 250 WHILE Timer0 < 250 WEND RESET Timer0 IF Pulselenght > 250 AND Pulselenght < 500 AND Timer1 < 20000 THEN 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 INPUT T Cts = 0 Gpbyteone = Split(t , Contents(1) , ",") IF Gpbyteone = 2 THEN CALL Ascent() 'go to servo waggle if short message T = Contents(2) IF Timeout < 80 THEN 'ie we had another message not long ago Gpsheading = VAL(t) Gpfloatone = Gpsheading - Oldheading 'so dont update IF Gpfloatone < -180 THEN Gpfloatone = Gpfloatone + 360 END IF IF Gpfloatone > 180 THEN Gpfloatone = Gpfloatone - 360 END IF Gpfloatone = Kalmangpsweight * Gpfloatone Heading = Heading + Gpfloatone END IF Oldheading = Heading 'but always store the past heading Timeout = 0 T = Contents(3) Targetheading = VAL(t) T = Contents(4) Servoenable = VAL(t) T = Contents(5) Gpbyteone = VAL(t) IF Gpbyteone = 1 THEN Enable Interrupts ELSE Disable Interrupts END IF Waitus 250 PRINT "#," ; Rate ; "," ; Temperature ; "," ; ERROR ; "," ; Heading ; "," ; Pulselenght ; "," ; Pulses END IF CALL Talktomelexis() Gpintegerone = Rate Gpintegerone = Gpintegerone - 1018 Gpfloatone = Gpintegerone Gpfloatone = Gpfloatone / 13.653 Realrate = Gpfloatone 'now in degrees per second gpfloatone=gpfloatone*kalmangyroweight filteredrate=propogation*filteredrate filteredrate=filteredrate+gpfloatone gpfloatone=servo*servogain filteredrate=filteredrate+gpfloatone gpfloatone=filteredrate/62.5 Heading = Heading + gpfloatone IF Heading > 360 THEN Heading = Heading - 360 END IF IF Heading < 0 THEN Heading = Heading + 360 END IF Gpfloatone = Targetheading - Heading 'heading offset IF Gpfloatone < -180 THEN Gpfloatone = Gpfloatone + 360 END IF IF Gpfloatone > 180 THEN Gpfloatone = Gpfloatone - 360 END IF Gpfloattwo = Gpfloatone * Pterm Realrate = Dterm * Realrate 'pid Integral = Integral + Heading Gpfloatone = Integral * Iterm IF Gpfloatone > 30 THEN Integral = 30 / Iterm 'wind up prevension IF Gpfloatone < -30 THEN Integral = -30 / Iterm Gpfloatone = Gpfloatone + Realrate Gpfloatone = Gpfloatone + Gpfloattwo Gpfloatone = Servogain * Gpfloatone IF Gpfloatone > 500 THEN Gpfloatone = 500 'limits IF Gpfloatone < -500 THEN Gpfloatone = -500 servo=gpfloatone 'store servo Gpintegerone = Gpfloatone Gpintegerone = Gpintegerone + Center Portd.5 = 1 IF Groundcontrol = 0 AND Servoenable = 1 THEN Pulseout , Portd , 5 , Gpintegerone 'only if not under ground control LOOP Edge: IF Groundcontrol = 1 THEN Servotwo = NOT Inputtwo IF Inputtwo = 0 THEN Pulselenght = Timer1 END IF Timer1 = 0 RETURN Copy: IF Groundcontrol = 1 THEN Servoone = NOT Inputone RETURN SUB Ascent() Integral = 0 'reset integral Waitus 250 PRINT "#," ; Rate ; "," ; Temperature DO Waitms 15 Pulseout , Portd , 5 , Center 'keep servo centered CALL Talktomelexis() IF Rts = 1 THEN 'check for comms Cts = 1 INPUT T Cts = 0 Gpbyteone = Split(t , Contents(1) , ",") IF Gpbyteone > 2 THEN EXIT SUB Waitus 250 PRINT "#," ; Rate ; "," ; Temperature ; "," ; ERROR Portd.5 = 1 FOR Gpbyteone = 0 TO 10 Pulseout , Portd , 5 , 5500 'wiggle servo Waitms 15 NEXT FOR Gpbyteone = 0 TO 10 Pulseout , Portd , 5 , 6500 Waitms 15 NEXT END IF LOOP END SUB 'Sub Ground() 'Groundcontrol = 1 'While Timer1 < 20000 And Pulselenght > 200 And Pulselenght < 600 'Servoone = Not Inputone 'Servotwo = Not Inputtwo 'Wend 'Servotwo = 1 'Servoone = 1 'Groundcontrol = 0 'End Sub SUB Talktomelexis() Clock = 0 Dout = 0 Slave = 0 ERROR = 0 Waitus 1 FOR Gpbyteone = 7 TO 0 STEP -1 Dout = Rateread.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 Rate = Makeint(lsb , Msb) Rate = Rate AND Maskword Shift Rate , Right , 1 IF Msb.6 = 1 THEN ERROR = 1 Slave = 0 Waitus 1 FOR Gpbyteone = 7 TO 0 STEP -1 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.1188989134.txt.gz · Last modified: 2008/07/19 23:32 (external edit)