UKHAS Wiki

UK High Altitude Society

User Tools

Site Tools

Trace:
projects:ukhas_glider_project:slave

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revisionPrevious revision
Next revision		Previous revision
projects:ukhas_glider_project:slave [2007/09/05 10:45] – laurencebprojects:ukhas_glider_project:slave [2008/07/19 23:33] (current) – external edit 127.0.0.1
Line 7: Line 7:
 $framesize = 64 $framesize = 64
 Const Servogain = 10 Const Servogain = 10
-Const Pterm = 1                               'needs to be -1 here? as gyro will be chip facing up, not down +Const Pterm = -0.25                                         'all -ive as a +ive pulse lenght causes +ive turn 
-Const Iterm = 0.001 +Const Iterm = -0.0003 
-Const Dterm = 0 +Const Dterm = -0.35 
-Const Kalmangpsweight = 1 +Const Kalmangpsweight = 0.5 
-const kalmangyroweight=0.05 +Const Kalmangyroweight = 0.5 
-const propogation=0.98*(1-kalmangyroweight) +Const Propogation = 0.98 *(1 -kalmangyroweight) 
-const servogain=-0.007*(1-kalmangyroweight)                        'need to check these with simcode +Const Servocontrolinput = 0.0015 *(1 -kalmangyroweight)     'need to check these with simcode 
-Const Center = 6000                                                           'needs to be checked with servo+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 Baud = 125000
 Config Portd.4 = Output Config Portd.4 = Output
Line 28: Line 31:
 Config Portc.2 = Output Config Portc.2 = Output
 Config Portc.3 = Output Config Portc.3 = Output
 +Config Watchdog = 1024
 'Config Spi = Soft , Din = Pinc.4 , Dout = Portc.5 , Ss = None , Clock = Portc.2 'Config Spi = Soft , Din = Pinc.4 , Dout = Portc.5 , Ss = None , Clock = Portc.2
 'Spiinit 'Spiinit
Line 34: Line 38:
 Dim Rate As Integer , Temperature As Integer , Lsb As Byte , Msb As Byte       'melexis 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 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 Firstupdate As Byte , Servoexcitecount As Byte          '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 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 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 U As Integer , Gpintegerone As Integer , Targetheading As Single , Gpsheading As Single , Heading As Single
 Dim Gpwordone As Word , Pulselenght As Word Dim Gpwordone As Word , Pulselenght As Word
Line 61: Line 64:
 'Declare Sub Ground() 'Declare Sub Ground()
 Declare Sub Talktomelexis() Declare Sub Talktomelexis()
-Declare Sub Ascent() +'Declare Sub Ascent() 
-On Pcint2 Edge +On Pcint0 Edge 
-On Pcint0 Copy+On Pcint2 Copy
 Set Pcifr.pcif2 Set Pcifr.pcif2
 Set Pcifr.pcif0 Set Pcifr.pcif0
 Enable Pcint2 Enable Pcint2
 Enable Pcint0 Enable Pcint0
 +Groundcontrol = 0
 Servotwo = 1 Servotwo = 1
 Servoone = 1 Servoone = 1
 S = 0 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 Timer1
 Start Timer0 Start Timer0
 Enable Interrupts Enable Interrupts
-Wait 2+Start Watchdog
  
 Do Do
- Incr Timeout + Reset Watchdog 
- If Timeout > 250 Then Timeout = 250 + Waitms 15 
- While Timer0 < 250+ 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  Wend
  Reset Timer0  Reset Timer0
- If Pulselenght > 250 And Pulselenght < 500 And Timer1 < 20000 Then+ If Pulselenght > 200 And Pulselenght < 600 And Timer1 < 20000 Then       'a valid and recent pulse
   Incr Pulses   Incr Pulses
  Else  Else
Line 95: Line 150:
 If Rts = 1 Then                                             'check for comms If Rts = 1 Then                                             'check for comms
  Cts = 1  Cts = 1
- Input T+ Inputbin Gpbyteone , Gpsheading , Targetheading , Servoenable , Enablegroundcontrol
  Cts = 0  Cts = 0
- Gpbyteone = Split(t , Contents(1) , ",") + If Firstupdate = 0 Or Firstupdate = 1 Then                 'ie we had another message not long ago 
- If Gpbyteone = 2 Then Call Ascent()                        'go to servo waggle if short message +  Gpfloatone = Gpsheading - Oldheading                      'so update the heading estimate 
- T = Contents(2) +  If Gpfloatone < -180 Then                                 'otherwise we dont bother as we dont know what was happening 
- If Timeout < 80 Then                                       'ie we had another message not long ago +   Gpfloatone = Gpfloatone + 360                            'one second ago
-  Gpsheading = Val(t) +
-  Gpfloatone = Gpsheading - Oldheading              'so dont update +
-  If Gpfloatone < -180 Then +
-   Gpfloatone = Gpfloatone + 360+
   End If   End If
   If Gpfloatone > 180 Then   If Gpfloatone > 180 Then
-   Gpfloatone = Gpfloatone - 360+   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   End If
-  Gpfloatone = Kalmangpsweight * Gpfloatone 
   Heading = Heading + Gpfloatone   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  End If
- Oldheading = Heading                                     'but always store the past heading + Oldheading = Heading                                       ' always store the past heading 
- Timeout = 0 + If Enablegroundcontrol = 1 Then                            'enable ground control
- T = Contents(3) +
- Targetheading = Val(t) +
- T = Contents(4) +
- Servoenable = Val(t) +
- T = Contents(5) +
- Gpbyteone = Val(t) +
- If Gpbyteone = 1 Then+
   Enable Interrupts   Enable Interrupts
  Else  Else
Line 126: Line 183:
  End If  End If
  Waitus 250  Waitus 250
- Print "#," ; Rate ; "," ; Temperature ; "," ; Error ; "," ; Heading ; "," ; Pulselenght ; "," ; Pulses+ Printbin Rate ; Temperature ; Error ; Heading ; Filteredrate ; Pulselenght ; Pulses
 End If End If
 Call Talktomelexis() Call Talktomelexis()
 Gpintegerone = Rate Gpintegerone = Rate
-Gpintegerone = Gpintegerone - 1018+Gpintegerone = Gpintegerone - 1009                          'center position
 Gpfloatone = Gpintegerone Gpfloatone = Gpintegerone
-Gpfloatone = Gpfloatone / 13.653+Gpfloatone = Gpfloatone / 6.8265
 Realrate = Gpfloatone                                       'now  in degrees per second Realrate = Gpfloatone                                       'now  in degrees per second
-gpfloatone=gpfloatone*kalmangyroweight +Gpfloatone = Gpfloatone * Kalmangyroweight                  'gyro term 
-filteredrate=propogation*filteredrate +Filteredrate = Propogation * Filteredrate                   'propogation term 
-filteredrate=filteredrate+gpfloatone +Filteredrate = Filteredrate + Gpfloatone                    'add them 
-gpfloatone=servo*servogain +Gpfloatone = Servo * Servocontrolinput                      'control term - need to use rx pwm if under ground control 
-filteredrate=filteredrate+gpfloatone +Filteredrate = Filteredrate + Gpfloatone                    'add them 
-gpfloatone=filteredrate/62.5 +Gpfloatone = Filteredrate / 62.5                            'numerical integration over timestep 
-Heading = Heading + gpfloatone +Heading = Heading + Gpfloatone 
-If Heading > 360 Then+If Heading > 180 Then                                       'get heading in correct range
  Heading = Heading - 360  Heading = Heading - 360
 End If End If
-If Heading < 0 Then+If Heading < -180 Then
  Heading = Heading + 360  Heading = Heading + 360
 End If End If
-Gpfloatone = Targetheading - Heading                        'heading offset +Gpfloatone = Heading - Targetheading                        'heading offset 
-If Gpfloatone < -180 Then+If Gpfloatone < -180 Then                                   'get it in the right range
  Gpfloatone = Gpfloatone + 360  Gpfloatone = Gpfloatone + 360
 End If End If
Line 154: Line 211:
  Gpfloatone = Gpfloatone - 360  Gpfloatone = Gpfloatone - 360
 End If End If
-Gpfloattwo = Gpfloatone * Pterm +Integral = Integral + Gpfloatone                            'i increment                                                     'pid 
-Realrate = Dterm * Realrate                                 'pid +Gpfloattwo = Gpfloatone * Pterm                             'P 
-Integral = Integral + Heading +Gpfloatone = Dterm * Filteredrate                           'D 
-Gpfloatone = Integral * Iterm +Gpfloatone = Gpfloatone + Gpfloattwo                        'add d and p terms 
-If Gpfloatone > 30 Then Integral = 30 / Iterm               'wind up prevension +If Gpfloatone > 30 Then Gpfloatone = 30                     'servo limits on d and p 
-If Gpfloatone < -30 Then Integral = -30 / Iterm +If Gpfloatone < -30 Then Gpfloatone = -30 
-Gpfloatone = Gpfloatone + Realrate +Gpfloattwo = Integral * Iterm                               'calculate I 
-Gpfloatone = Gpfloatone + Gpfloattwo +If Gpfloattwo > 25 Then Integral = 25 / Iterm               'wind up prevension 
-Gpfloatone = Servogain * Gpfloatone +If Gpfloattwo < -25 Then Integral = -25 / Iterm 
-If Gpfloatone > 500 Then Gpfloatone = 500                   'limits +Gpfloatone = Gpfloatone + Gpfloattwo                        'add I term 
-If Gpfloatone < -500 Then Gpfloatone = -500 +Gpfloatone = Servogain * Gpfloatone                         'servo dependant variable 
-servo=gpfloatone                                                        'store servo+Servo = Gpfloatone                                          'store servo for control input
 Gpintegerone = Gpfloatone Gpintegerone = Gpfloatone
-Gpintegerone = Gpintegerone + Center +If Groundcontrol = 1 Then 
-Portd.5 = 1 + Gpfloatone = Pulselenght * Conversionfactor 
-If Groundcontrol = 0 And Servoenable = 1 Then Pulseout , Portd , 5 , Gpintegerone       'only if not under ground control+ 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 Loop
  
 Edge: Edge:
-If Groundcontrol = 1 Then Servotwo = Not Inputtwo +If Groundcontrol = 1 Then Servoone = Not Inputone           'copy 
-If Inputtwo = 0 Then +If Inputone = 0 Then 
- Pulselenght = Timer1+ Pulselenght = Timer1                                       'read pwm
 End If End If
 Timer1 = 0 Timer1 = 0
Line 181: Line 244:
  
 Copy: Copy:
-If Groundcontrol = 1 Then Servoone = Not Inputone+If Groundcontrol = 1 Then Servotwo = Not Inputtwo           'copy
 Return 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() Sub Talktomelexis()
Line 233: Line 253:
  Error = 0  Error = 0
  Waitus 1  Waitus 1
- For Gpbyteone = 7 To 0 Step -1+ For Gpbyteone = 7 To 0 Step -1                             'spi out
   Dout = Rateread.gpbyteone   Dout = Rateread.gpbyteone
   Waitus 1   Waitus 1
Line 240: Line 260:
   Clock = 0   Clock = 0
  Next  Next
- For Gpbyteone = 15 To 0 Step -1+ For Gpbyteone = 15 To 0 Step -1                            'extra clock cycles is in datasheet
   Waitus 1   Waitus 1
   Clock = 1   Clock = 1
Line 250: Line 270:
  Waitus 300  Waitus 300
  Slave = 0  Slave = 0
- For Gpbyteone = 7 To 0 Step -1+ For Gpbyteone = 7 To 0 Step -1                             'spi out
   Dout = Readmelexis.gpbyteone   Dout = Readmelexis.gpbyteone
   Waitus 1   Waitus 1
Line 257: Line 277:
   Clock = 0   Clock = 0
  Next  Next
- For Gpbyteone = 7 To 0 Step -1+ For Gpbyteone = 7 To 0 Step -1                             'data in
   Waitus 1   Waitus 1
   Clock = 1   Clock = 1
Line 264: Line 284:
   Clock = 0   Clock = 0
  Next  Next
- For Gpbyteone = 7 To 0 Step -1+ For Gpbyteone = 7 To 0 Step -1                             'another byte in
   Waitus 1   Waitus 1
   Clock = 1   Clock = 1
Line 273: Line 293:
  Waitus 1  Waitus 1
  Slave = 1  Slave = 1
- Rate = Makeint(lsb , Msb) + Rate = Makeint(lsb , Msb)                                  'create integer 
- Rate = Rate And Maskword + Rate = Rate And Maskword                                   'mask it 
- Shift Rate , Right , 1 + Shift Rate , Right , 1                                     'shift it 
- If Msb.6 = 1 Then Error = 1+ If Msb.6 = 1 Then Error = 1                                'check selftest feature
  Slave = 0  Slave = 0
  Waitus 1  Waitus 1
- For Gpbyteone = 7 To 0 Step -1+ For Gpbyteone = 7 To 0 Step -1                             'do the same sort of thing for temp
   Dout = Tempread.gpbyteone   Dout = Tempread.gpbyteone
   Waitus 1   Waitus 1
projects/ukhas_glider_project/slave.1188989134.txt.gz · Last modified: 2008/07/19 23:32 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki