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 CONST Iterm = -0.001 CONST Dterm = -1 CONST Kalmangpsweight = 0.6 CONST Kalmangyroweight = 0.1 CONST Propogation = 0.98 *(1 -kalmangyroweight) CONST Servocontrolinput = 0.0015 *(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 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 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 Start Watchdog 'While Rts = 0 'Wend 'Cts = 1 'Input T 'Cts = 0 'Gpbyteone = Split(t , Contents(1) , ",") 'If Gpbyteone = 2 Then Call Ascent() DO RESET Watchdog Incr Timeout IF Timeout > 250 THEN Timeout = 250 WHILE Timer0 < 250 'wait for 16ms to elapse WEND RESET Timer0 IF Pulselenght > 250 AND Pulselenght < 500 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 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 update the heading estimate 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 ' always store the past heading Timeout = 0 'reset timeout as we just got a message T = Contents(3) Targetheading = VAL(t) T = Contents(4) Servoenable = VAL(t) IF Contents(5) = "1" THEN 'enable ground control 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 '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 > 360 THEN 'get heading in correct range Heading = Heading - 360 END IF IF Heading < 0 THEN Heading = Heading + 360 END IF Gpfloatone = Targetheading - Heading '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 'pid Gpfloattwo = Gpfloatone * Pterm 'p Realrate = Dterm * Filteredrate 'd reuses the realrate variable Integral = Integral + Heading 'i Gpfloatone = Integral * Iterm IF Gpfloatone > 30 THEN Integral = 30 / Iterm 'wind up prevension IF Gpfloatone < -30 THEN Integral = -30 / Iterm Gpfloatone = Gpfloatone + Realrate 'add d and i terms Gpfloatone = Gpfloatone + Gpfloattwo 'add on the p Gpfloatone = Servogain * Gpfloatone 'servo dependant variable IF Gpfloatone > 500 THEN Gpfloatone = 500 'servo limits IF Gpfloatone < -500 THEN Gpfloatone = -500 Servo = Gpfloatone 'store servo for control input Gpintegerone = Gpfloatone Gpintegerone = Gpintegerone + Center 'add on pwm center value Servoone = 1 'set pwm high 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 'copy IF Inputtwo = 0 THEN Pulselenght = Timer1 'read pwm END IF Timer1 = 0 RETURN Copy: IF Groundcontrol = 1 THEN Servoone = NOT Inputone 'copy RETURN SUB Ascent() Integral = 0 'reset integral Disable Interrupts Groundcontrol = 0 CALL Talktomelexis() 'get values, also delays us by a bit to sync comms PRINT "#," ; Rate ; "," ; Temperature ; "," ; ERROR 'continue comms DO RESET Watchdog Waitms 15 Pulseout , Portd , 5 , Center 'keep servo centered CALL Talktomelexis() 'check gyro IF Rts = 1 THEN 'check for comms Cts = 1 INPUT T Cts = 0 Gpbyteone = Split(t , Contents(1) , ",") IF Gpbyteone > 2 THEN 'if we get a descent command Timeout = 250 'means we treat the first gps update correctly EXIT SUB END IF Waitus 300 PRINT "#," ; Rate ; "," ; Temperature ; "," ; ERROR Servoone = 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 '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.1189464735.txt.gz · Last modified: 2008/07/19 23:32 (external edit)