{{
  Image Stabilizer - iStabilizer-V2_3.SPIN 
  Author: Earl Foster                     
  Modifed Date: 11/09/2008
}}

CON
    _clkmode = xtal1 + pll16x                           
    _xinfreq = 5_000_000                                

    xServo = 0                                        
    yServo = 8
    CS = 20
    DIO = 21
    CLK = 22

    error = 30        'Error factor (10 = 1 degree)

    xCenter = 1420    'Will change depending on servo
    yCenter = 1425    'Each servo could have different center points
    multipler = 10    'Determined by dividing Servo Range by Degree of Movement 600/60

VAR
    long x, y, pause, pulsout, xprev, yprev

OBJ
    H48C  : "H48C Tri-Axis Accelerometer"
    debug : "fullduplexserialplus"

PUB Main

   Init_Debug 
   
   pause := clkfreq/1_000         'Pause is equal to 1 ms
   pulsout := clkfreq/1_000_000   'Provides better granularity for servo positions

   dira[0..8]~~
   outa[0..8]~

   xprev := xCenter
   yprev := yCenter
   H48C.start(CS,DIO,CLK)         'Starts reading the accelerometer
     repeat
        x := H48C.ThetaC*45/1024
        y := H48C.ThetaB*45/1024
        if x =< 90                 'Swing in opposite direction from 360 - 300 degrees
           x := xCenter - (x*multipler) #> 1020
           if (x > xprev+error) or (x < xprev-error)
              position(xServo,x)
              xprev := x
           else
              position(xServo, xprev)
              x := xprev
        elseif x > 270 and x =< 360     'Swing in opposite direction from 0 - 60 degrees
           x := xCenter + ((360-x)*multipler) <# 1820
           if x > xprev+error or x < xprev-error
              position(xServo,x)
              xprev := x
           else
              position(xServo, xprev)
              x := xprev
        else
           position(xServo, xCenter)
           x:= xCenter
           
        if y =< 90
           y:= yCenter - ((90-y)*multipler) #> 1025 
           if (y > yprev+error) or (y < yprev-error)
              position(yServo,y)    
              yprev := y
           else
              position(yServo, yprev)
              y := yprev
        elseif y > 90 and y =< 180
           y:= yCenter + ((y-90)*multipler) <# 1825 
           if (y > yprev+error) or (y < yprev-error)
              position(yServo,y)    
              yprev := y
           else
              position(yServo, yprev)
              y := yprev
        else
           position(yServo, yCenter)
           y := yCenter
               
        waitcnt(pause*50 +cnt)
        debug.dec(x)
        debug.str(string("   "))
        debug.dec(y)
        debug.str(string("   "))
        debug.tx(13)
        
pub position(servo, axis)
{{
  Positions the servos to the appropriate position
}}
   outa[servo]~~
   waitcnt(pulsout * axis + cnt)
   outa[servo]~
   waitcnt(pause * 20 + cnt)

pub Init_DEBUG
{{
  Starts up serial communication with a computer and displays
  information to a terminal screen
}}
  debug.start(31, 30, 0, 9600)
  debug.str(string(13,"Starting Program", 13))
  debug.str(string(13,"Running iStablizerv2_3",13))
  waitcnt(clkfreq + cnt)