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/10 22:52] – 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 +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 Lowerpwm = Center - 300 
 +Const Upperpwm = Center + 300
 Baud = 125000 Baud = 125000
 Config Portd.4 = Output Config Portd.4 = Output
Line 35: 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 , Timeout As Byte , Error As Byte+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 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 62: 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 Start Watchdog
-'While Rts = 0 
-'Wend 
-'Cts = 1 
-'Input T 
-'Cts = 0 
-'Gpbyteone = Split(t , Contents(1) , ",") 
-'If Gpbyteone = 2 Then Call Ascent() 
  
 Do Do
  Reset Watchdog  Reset Watchdog
- Incr Timeout + Waitms 15 
- If Timeout > 250 Then Timeout = 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  While Timer0 < 250                                         'wait for 16ms to elapse
  Wend  Wend
  Reset Timer0  Reset Timer0
- If Pulselenght > 250 And Pulselenght < 500 And Timer1 < 20000 Then       'a valid and recent pulse+ If Pulselenght > 200 And Pulselenght < 600 And Timer1 < 20000 Then       'a valid and recent pulse
   Incr Pulses   Incr Pulses
  Else  Else
Line 105: 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 +
- 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   Gpfloatone = Gpsheading - Oldheading                      'so update the heading estimate
-  If Gpfloatone < -180 Then +  If Gpfloatone < -180 Then                                 'otherwise we dont bother as we dont know what was happening 
-   Gpfloatone = Gpfloatone + 360+   Gpfloatone = Gpfloatone + 360                            'one second ago
   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                                       ' always store the past heading  Oldheading = Heading                                       ' always store the past heading
- Timeout = 0                                                'reset timeout as we just got a message + If Enablegroundcontrol = 1 Then                            'enable ground control
- T = Contents(3) +
- Targetheading = Val(t) +
- T = Contents(4) +
- Servoenable = Val(t) +
- If Contents(5) = "1" Then                                  'enable ground control+
   Enable Interrupts   Enable Interrupts
  Else  Else
Line 134: 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                  'gyro term Gpfloatone = Gpfloatone * Kalmangyroweight                  'gyro term
Line 149: Line 198:
 Gpfloatone = Filteredrate / 62.5                            'numerical integration over timestep Gpfloatone = Filteredrate / 62.5                            'numerical integration over timestep
 Heading = Heading + Gpfloatone Heading = Heading + Gpfloatone
-If Heading > 360 Then                                       'get heading in correct range+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                                   'get it in the right range If Gpfloatone < -180 Then                                   'get it in the right range
  Gpfloatone = Gpfloatone + 360  Gpfloatone = Gpfloatone + 360
Line 161: 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 + Heading                               '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
 +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 Gpintegerone = Gpintegerone + Center                        'add on pwm center value
-Servoone = 1                                                'set pwm high +If Groundcontrol = 0 And Servoenable = 1 Then 
-If Groundcontrol = 0 And Servoenable = 1 Then Pulseout , Portd , 5 , Gpintegerone       'only if not under ground control+ 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           'copy +If Groundcontrol = 1 Then Servoone = Not Inputone           'copy 
-If Inputtwo = 0 Then+If Inputone = 0 Then
  Pulselenght = Timer1                                       'read pwm  Pulselenght = Timer1                                       'read pwm
 End If End If
Line 189: Line 244:
  
 Copy: Copy:
-If Groundcontrol = 1 Then Servoone = Not Inputone           'copy+If Groundcontrol = 1 Then Servotwo = Not Inputtwo           'copy
 Return 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() Sub Talktomelexis()
projects/ukhas_glider_project/slave.1189464735.txt.gz · Last modified: 2008/07/19 23:32 (external edit)
Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki