//------------------------------------------------
// NXTway - Philo - www.philohome.com - 6/5/2006
//------------------------------------------------
dseg segment

// Sensor values
NVal word
offset word
err sdword
errold sdword
errdiff sdword
errint sdword

// Motor values
theUF byte
thePower sbyte
theOM byte OUT_MODE_MOTORON+OUT_MODE_BRAKE
theRM byte OUT_REGMODE_IDLE
theRS byte OUT_RUNSTATE_RUNNING
thePorts byte[] OUT_B, OUT_C  // motors B and C

// pid coeffs
kp sdword 30
kd sdword 35
ki sdword 5
scale sdword 45

// pid value
pid sdword

//temp var
temp sdword

// timer vars
thenTick dword
nowTick dword

dseg ends

thread main

	setin	IN_TYPE_LIGHT_ACTIVE, IN_2, Type

  // initialize motors
  set thePower, 0
	set theUF, UF_UPDATE_SPEED+UF_UPDATE_MODE
	setout thePorts, OutputMode, theOM, RegMode, theRM, RunState, theRS, UpdateFlags, theUF, Power, thePower
	set theUF, UF_UPDATE_SPEED
	
  // wait a bit to let sensor stabilize
	gettick nowTick
	add	thenTick, nowTick, 100 // wait 100 ms
Waiting:
	gettick nowTick
	brcmp	LT, Waiting, nowTick, thenTick

  // reads center value. NXTway must be balanced.
  getin	NVal, IN_2, NormalizedValue // More precise than PercentValue
  mov offset, NVal

Forever:
	getin	NVal, IN_2, NormalizedValue // read sensor values

  sub err, NVal, offset // Substract center value
  brtst GT, ErrPos, err // Linearize value
  mul err, err, 16      // (less variation if far from surface)
  div err, err, 10
ErrPos:
  sub errdiff, err, errold  // Compute differential error
  mov errold, err
  
  add errint, errint, err   // Compute integral error
  mul errint, errint, 2     // (with fast damping)
  div errint, errint, 3
  
  mul pid, kd, errdiff      // Differential component
  mul temp, kp, err         // Proportionnnal component

  add pid, pid, temp        // Add Diff+Prop
  mul temp, ki, errint      // Integral component
  add pid, pid, temp        // Add int
  div pid, pid, scale       // Scale PID value
  
  // saturate over 100 and under -100
  brcmp LT, under100, pid, 100
  mov pid, 100
under100:
  brcmp GT, overMin100, pid, -100
  mov pid, -100
overMin100:

  mov thePower, pid          // Update motor power
	setout thePorts, UpdateFlags, theUF, Power, thePower
  jmp Forever
  exit
endt