TiltRotate in Sequence

[TiltRotate]

set _FDegToRad as #F1.0 / #F57.29578
set _FXMin as #F-20.0
set _FXMax as #F20.0
set _FYMin as #F-20.0
set _FYMax as #F20.0
set _FZMin as #F-20.0
set _FZMax as #F20.0
set _FXLines as #F10.0
set _FXSublines as #F10.0
set _FYLines as #F10.0
set _FYSublines as #F10.0

set _FTilt as *FTiltSensor * ^FTiltMag
set _FRota as *FRotaSensor * ^FRotaMag

set _FRadTilt as _FTilt * _FDegToRad
set _FCosTilt from RadCos _FRadTilt
set _FSinTilt from RadSin _FRadTilt
set _FRadRota as _FRota * _FDegToRad
set _FCosRota from RadCos _FRadRota
set _FSinRota from RadSin _FRadRota
set _FXMean as _FXMax + _FXMin
set _FXMean as _FXMean / #F2.0
set _FYMean as _FYMax + _FYMin
set _FYMean as _FYMean / #F2.0
set _FZMean as _FZMax + _FZMin
set _FZMean as _FZMean / #F2.0
set _FXDiff as _FXMax - _FXMin
set _FYDiff as _FYMax - _FYMin
set _FZDiff as _FZMax - _FZMin

set _FXStep as _FXLines - #F1.0
set _FXStep as _FXDiff / _FXStep
set _FXSubstep as _FXSublines - #F1.0
set _FXSubstep as _FXDiff / _FXSubstep

set _FYStep as _FYLines - #F1.0
set _FYStep as _FYDiff / _FYStep
set _FYSubstep as _FYSublines - #F1.0
set _FYSubstep as _FYDiff / _FYSubstep

set _FXMaxx as _FXMax + #F0.1
set _FYMaxx as _FYMax + #F0.1

step _FY from _FYMin to _FYMaxx increment _FYStep
  step _FX from _FXMin to _FXMaxx increment _FXSubstep
    if _FX == _FXMin
      gosub Calculate
    endif
    copy _FXPlot to _PLast.X
    copy _FYPlot to _PLast.Y
    gosub Calculate
    copy _FXPlot to _PThis.X
    copy _FYPlot to _PThis.Y
    call DrawLine _PLast _PThis #C0,255,0
  endstep
endstep

step _FX from _FXMin to _FXMaxx increment _FXStep
  step _FY from _FYMin to _FYMaxx increment _FYSubstep
    if _FY == _FYMin
      gosub Calculate
    endif
    copy _FXPlot to _PLast.X
    copy _FYPlot to _PLast.Y
    gosub Calculate
    copy _FXPlot to _PThis.X
    copy _FYPlot to _PThis.Y
    call DrawLine _PLast _PThis #C0,255,255
  endstep
endstep

sub Calculate
  set _FZX as _FX * _FX
  set _FZY as _FY * _FY
  set _FZ as _FZX + _FZY
  set _FZ from SquareRoot _FZ
  set _FZ from RadSin _FZ
  set _FZ2 as _FZ - _FZMean
  set _FZ2 as _FZ2 / _FZDiff
  set _FX2 as _FX - _FXMean
  set _FX2 as _FX2 / _FXDiff
  set _FY2 as _FY - _FYMean
  set _FY2 as _FY2 / _FYDiff
  set _FX3X as _FX2 * _FCosRota
  set _FX3Y as _FY2 * _FSinRota
  set _FX3 as _FX3X - _FX3Y
  set _FY3X as _FX2 * _FSinRota
  set _FY3Y as _FY2 * _FCosRota
  set _FY3Z as _FZ2 * _FCosTilt
  set _FY3XY as _FY3X + _FY3Y
  set _FY3S as _FY3XY * _FSinTilt
  set _FY3 as _FY3Z - _FY3S
  set _FXPlot as ^FMagX * _FX3
  set _FXPlot as ^FCentreX + _FXPlot
  set _FYPlot as ^FMagY * _FY3
  set _FYPlot as ^FCentreY + _FYPlot
endsub