'Set-up a loop, as close to 1ms as possible
'Used this method because it can be modified (500us for example)
timer=0
 schd=0.9794
 do
   do:loop until timer>schd:inc schd,0.9794
   strt=timer 'simply for keeping an eye on loop execution time
   
   'Read the motor's encoder (LS7366)
   pin(85)=0
   spi write 1,&h60
   spi read 4,psns()
   pin(85)=1
   apos=(psns(1)<<24) or (psns(2)<<16) or (psns(3)<<8) or psns(4)
   'apos (actual position is signed 32bit)
   if apos and &h80000000 then inc apos, - &hffffffff
   'positionerror = command_position(setpoint)-actual_position
   perr=cpos-apos
   'if using the integral, begin to accumulate and
   'keep within the anti-wind-up limit
   if ki>0 then inc intg,perr
   if intg>ilim then intg=ilim
   if intg<-ilim then intg=-ilim
   
   'standard PID
   'mcmd=50 is the PWM offset for locked antiphase where
   '50% duty-cycle is zero motor current
   mcmd=50+((kp*perr)+(kd*(perr-prvr))+(ki*intg))
   'tl=torque-limit.
   if tl then
     if mcmd<tl then mcmd=tl
     if mcmd>tlhi then mcmd=tlhi
   end if
   
   'LMD18200 datasheet advises to avoid 100% and 0% duty-cycle(whatever)
   if mcmd<0.2 then mcmd=0.2
   if mcmd>99.8 then mcmd=99.8
   'if motor-disable commanded then set duty-cycle to 50%
   if moff then mcmd = 50
   pwm 1,18000,mcmd
   'copy positionerror to previouserror for next kd calc
   prvr=perr
   fini=timer
 loop  

'Simple Proportional heat control
'Objectives:
'
'1) Immune to instability
'2) Rapid calibration

'The premise being that hovering around a setpoint isn't
'necessary for heating.That's where we get instability.
'The important things are the fact that it's a proportional
'control and that it's consistent
'Here we have a dummy target that will never be reached because the
'loop gain won't be sufficient. Shooting for Desired+dummy but calibrated to
'output zero-heat when desired is reached.
'The command will fade away and the only
'calibration required is to tweak either the Kp or the Dummy_Target_Temp_Offset
'to have the room temperature match the desired temp.
'At Desired Temp, there will still be a differential but not enough to produce
'an output signal

dim float Desired_Temp
dim float Dummy_Target_Temp_Offset
dim float Actual_Temp
dim float Differential
dim float Heat_Command

dim float Kp

'Test values

Kp=0.4  'stab in the dark
Desired_Temp=20
Dummy_Target_Temp_Offset=4

do
 Actual_Temp=[sensor_reading]
 Differential=(Desired_Temp + Dummy_Target_Temp_Offset) - (Actual_Temp)
 Heat_Command=Kp*Differential
 [output_heat_command]
 pause 10000 'stab in the dark
loop

' PID Heating control
' Well just PI for now
' D to come - hopefully
'
dim float Desired_Temp
dim float Dummy_Target_Temp_Offset
dim float Actual_Temp
dim float Differential 'Proportional
dim float Heat_Command
dim float Integral

dim integer Looptime

dim float Kp
dim float Ki

'Test values
Looptime=10000 ' 10 seconds for now
Ki=0    'Start with zero, increase in small steps while tuning
Kp=0.4  'stab in the dark
Desired_Temp=20
Dummy_Target_Temp_Offset=4 ' zero if full PID?

do
Actual_Temp=[sensor_reading]
Temperr = Desired_Temp - Actual_Temp

' INTEGRAL
' Next line Could be omitted, if Ki= 0 it then has no effect on Heat_command
if ki>0 then inc Integral,Temperr
if Integral>Desired_Temp then Integral=Desired_Temp 'Don't go higher than Desired Temp
'  if Integral<-Desired_Temp then Integral=-Desired_Temp 'Not needed +ve only

' PROPORTIONAL
Differential=(Desired_Temp + Dummy_Target_Temp_Offset) - (Actual_Temp)

' HEAT COMMAND
Heat_Command=Kp*Differential + Ki*Integral
' [output_heat_command]
pause Looptime
loop