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 by 127.0.0.1