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/15 23:21] – 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                                            'all -ive as a +ive pulse lenght causes +ive turn +Const Pterm = -0.25                                         'all -ive as a +ive pulse lenght causes +ive turn 
-Const Iterm = -0.001 +Const Iterm = -0.0003 
-Const Dterm = -1 +Const Dterm = -0.35 
-Const Kalmangpsweight = 0.6 +Const Kalmangpsweight = 0.5 
-Const Kalmangyroweight = 0.1+Const Kalmangyroweight = 0.5
 Const Propogation = 0.98 *(1 -kalmangyroweight) Const Propogation = 0.98 *(1 -kalmangyroweight)
 Const Servocontrolinput = 0.0015 *(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 Conversionfactor = 16                                 'pwm timer counter is 16 times slower than sw pwm
 Const Lowerpwm = Center - 300 Const Lowerpwm = Center - 300
Line 38: 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 Firstupdate As Bit , Servoexcitecount As Byte           'rest+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 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 , Error As Byte , Enablegroundcontrol As Byte
Line 76: Line 76:
 S = 0 S = 0
 Integral = 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
Line 105: Line 109:
   Cts = 0   Cts = 0
   If Gpbyteone = 1 Then                                     'if we get a descent command   If Gpbyteone = 1 Then                                     'if we get a descent command
-    Firstupdate = 1                                         'means we treat the first gps update correctly+    Firstupdate = 2                                         'means we treat the first gps update correctly
     Waitus 300     Waitus 300
     Printbin Rate ; Temperature ; Error ; Heading ; Filteredrate ; Pulselenght ; Pulses       'removes possible cause of a hang     Printbin Rate ; Temperature ; Error ; Heading ; Filteredrate ; Pulselenght ; Pulses       'removes possible cause of a hang
Line 148: Line 152:
  Inputbin Gpbyteone , Gpsheading , Targetheading , Servoenable , Enablegroundcontrol  Inputbin Gpbyteone , Gpsheading , Targetheading , Servoenable , Enablegroundcontrol
  Cts = 0  Cts = 0
- If Firstupdate = 0 Then                                    'ie we had another message not long ago+ If Firstupdate = 0 Or Firstupdate = 1 Then                 'ie we had another message not long ago
   Gpfloatone = Gpsheading - Oldheading                      'so update the heading estimate   Gpfloatone = Gpsheading - Oldheading                      'so update the heading estimate
   If Gpfloatone < -180 Then                                 'otherwise we dont bother as we dont know what was happening   If Gpfloatone < -180 Then                                 'otherwise we dont bother as we dont know what was happening
Line 156: Line 160:
    Gpfloatone = Gpfloatone - 360                            'get it in the right range    Gpfloatone = Gpfloatone - 360                            'get it in the right range
   End If   End If
-  Gpfloatone = Kalmangpsweight * Gpfloatone+  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   Heading = Heading + Gpfloatone
   If Heading > 180 Then                                     'get heading in correct range   If Heading > 180 Then                                     'get heading in correct range
Line 164: Line 172:
    Heading = Heading + 360    Heading = Heading + 360
   End If   End If
- Else + Else                                                       'ie if firstupdate=2 
-  Firstupdate = 0+  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                                       ' always store the past heading  Oldheading = Heading                                       ' always store the past heading
Line 178: Line 187:
 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                  'gyro term Gpfloatone = Gpfloatone * Kalmangyroweight                  'gyro term
Line 201: Line 210:
 If Gpfloatone > 180 Then If Gpfloatone > 180 Then
  Gpfloatone = Gpfloatone - 360  Gpfloatone = Gpfloatone - 360
-End If                                                      'pid +End If 
-Gpfloattwo = Gpfloatone * Pterm                             'p +Integral = Integral + Gpfloatone                            'i increment                                                     'pid 
-Realrate = Dterm * Filteredrate                             'd   reuses the realrate variable +Gpfloattwo = Gpfloatone * Pterm                             'P 
-Integral = Integral + Gpfloatone                            'i +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                          'add d and i terms +Gpfloattwo = Integral * Iterm                               'calculate I 
-Gpfloatone = Gpfloatone + Gpfloattwo                        'add on the p+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 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 Servo = Gpfloatone                                          'store servo for control input
 Gpintegerone = Gpfloatone Gpintegerone = Gpfloatone
projects/ukhas_glider_project/slave.1189898479.txt.gz · Last modified: 2008/07/19 23:32 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki