' ------------------------------------------------------------------------------
' ------------------------------------------------------------------------------
' ----[This file receive arrays from LabView                                 ]--
' ----[This happens in a loop. For each element of both arrays, there is a   ]--
' ----[step.                                                                 ]--
' ----[A digital PID controller is implemented                               ]--
' ----[Once the upload is done, the control loop is starded and the pressure ]--
' ----[time values are send to LabView via the serial port buffer.           ]--                                                                
' ------------------------------------------------------------------------------
' ------------------------------------------------------------------------------
' ----[Guillaume Bugnard      ]-------------------------------------------------
' ----[27/08/2007             ]-------------------------------------------------
' ----[Masterthesis ETH Zurich]-------------------------------------------------
' ------------------------------------------------------------------------------
' ------------------------------------------------------------------------------

' ------------------------------------------------------------------------------
' ----[Define the properties of the micro-controller]---------------------------
' ------------------------------------------------------------------------------
$regfile = "m64def.dat"                                     ' def file for ATmega16
$baud = 9600                                                '9600
$crystal = 16000000                                         ' 16MHz cristal

' ------------------------------------------------------------------------------
' ----[Configure the watchdog]--------------------------------------------------
' ------------------------------------------------------------------------------
Config Watchdog = 128                                       'Reset program after 16[ms]
Stop Watchdog

' ------------------------------------------------------------------------------
' ----[Define the internal memory allocation]-----------------------------------
' ------------------------------------------------------------------------------
$hwstack = 256
$swstack = 256
$framesize = 256

' ------------------------------------------------------------------------------
' ----[Open COM1 for data transfer to PC]-----------------------------------
' ------------------------------------------------------------------------------
Open "comd.1:9600,8,n,1,inverted" For Output As #1

' ------------------------------------------------------------------------------
' ----[Define the port for input in the terminal]-------------------------------
' ------------------------------------------------------------------------------
Config Porte.0 = Input
Config Porte.1 = Output

Config Serialin = Buffered , Size = 128
Config Serialout = Buffered , Size = 128

' ------------------------------------------------------------------------------
' ----[Configure the LCD screen]------------------------------------------------
' ------------------------------------------------------------------------------
Config Lcdpin = Pin , Db4 = Portf.3 , Db5 = Portf.2 , Db6 = Portf.1 , Db7 = Portf.0 , E = Porta.1 , Rs = Porta.2
Config Lcd = 20 * 4
Config Lcdbus = 4

' ------------------------------------------------------------------------------
' ----[Start the Analog->Digital converter]-------------------------------------
' ------------------------------------------------------------------------------
Config Adc = Single , Prescaler = 32 , Reference = Internal ,
Start Adc

' ------------------------------------------------------------------------------
' ----[Configure the PWM signal maker]------------------------------------------
' ------------------------------------------------------------------------------
' set direction of Ports.  0=input,  1=output:

Ddra = &B00001110                                           ' set portA direction
Ddrb = &B10010000                                           ' set portB direction
'Ddrc = &B11111111                      ' PortC = Output  (Addres/data-bus)
'Ddrd = &B11111110                      ' set portD direction
Ddrf = &B00001111
Config Timer0 = Pwm , Prescale = 64 , Compare Pwm = Clear Down

' ------------------------------------------------------------------------------
' ----[Declare variables]-------------------------------------------------------
' ------------------------------------------------------------------------------
'1. For the timer2 counter
Dim Cnt As Word
Dim T As Single
T = 0.0
Dim Intervaltime As Integer
Intervaltime = 16.0                                         'interval time in [ms]
'Dim Stopbyte As Byte
'Stopbyte = 0

'2. For the Upload of the pressure law arrays
Dim Arraydim As Integer
Dim Buffer As String * 10
Arraydim = 100
Dim Arraytime(100) As Single
Dim Arraypressure(100) As Single
Dim Dimensionlaw As Integer
Dim Timeend As Single                                       'End of the experiment

Dim N As Integer

'3. For the results (interpolated pressure)
Dim J As Integer
J = 1
Dim Numberstep As Integer
Numberstep = Arraydim

'4. For the transfer of the data from the controller to the PC
Dim K As Integer

'5. For the import of the pressure value from the sensor
Dim Druckbuffer As Word
Dim Druck As Single
Dim Drucktemp As Single
Dim Gainpressure As Single

'6. For the control signal
Dim R As Single
R = 0.0
Dim U As Single
U = 0.0
Dim Y As Single
Y = 0.0
Dim E1 As Single
Dim E2 As Single
Dim E3 As Single
E1 = 0.0
E2 = 0.0
E3 = 0.0

'7. PID parameters
Dim Kp As Single                                            'P parameter
Dim Ki As Single                                            'I parameter
Dim Kd As Single                                            'D parameter

Dim Timesampling As Single
Timesampling = Intervaltime / 1000.0

Dim Sumik As Single
Sumik = 0

'8. Motor parameters
Dim Direction As Byte
Direction = 0

'9. Pressure and time number to string conversion

'10. Interrupts
Dim Tim2isrbit As Byte                                      'Interrupter bit

'11. Number of cycles
Dim Numbercycle As Integer
Dim Cyclecounter As Integer
Cyclecounter = 0


' ------------------------------------------------------------------------------
' ----[Configure timer]---------------------------------------------------------
' ------------------------------------------------------------------------------
Const Timer2reload = 250
Const Timerstart = 6
Config Timer2 = Timer , Prescale = 1024
Load Timer2 , Timer2reload
On Ovf2 Tim2_isr
Enable Ovf2
Enable Interrupts
'start timer2
Stop Timer2
'Add your code here

'Timer value explination
' The timer is a 8Bit timer, it overflows when the timer reaches 256
' The AVR is running at 16000000Hz, the prescaler is 1024
' Each tick is 0.064 ms  -  (1 / CPUSpeed in KHz ) * Prescaler
' The timer needs  250 ticks to reach the required time  ( 250 * 0.064 = 16ms)
' The start value for the timer must be set to 6 so that it will overflow at 256 after 250 ticks
' NOTE: The load command does the inversion for you (256-value or 65536-value)

' ------------------------------------------------------------------------------
' ----[Declare the functions]---------------------------------------------------
' ------------------------------------------------------------------------------
'1. Declare the function which looks for the index of the wanted value
Declare Function Pinterpolated(timein As Single) As Single

' ------------------------------------------------------------------------------
' ----[A. Main Program]...------------------------------------------------------
' ------------------------------------------------------------------------------
Main:
   '1. Hello message
   '----------------
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "********************"
   Locate 2 , 1 : Lcd "*  Control box 3.3 *"
   Locate 3 , 1 : Lcd "*    G. Bugnard    *"
   Locate 4 , 1 : Lcd "********************"

   '1.1 The program waits until LabView send a carriage return (frame 1.2) to
   '    confirm that the buffer has been flushed.
   Buffer = Waitkey()
   '1.2 Send a keyword to LabView to tell the box is ready to import the
   '    pressure law.
   Print "Ready"
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "Ready"
   '2. Read the Kp value
   '--------------------
   '2.1 Once LabView is ready, the box receive the value for the R0 parameter
   Input Buffer
   Kp = Val(buffer)
   '2.2 The value is printed on the LCD screen
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "Kp = " ; Kp
   Waitms 1000
   '2.3 Once done, the keyword R0ok is sent to LabView
   Print "Kpok"
   '3. Read the Ki value
   '--------------------
   '3.1 Once LabView is ready, the box receive the value for the Ki parameter
   Input Buffer
   Ki = Val(buffer)
   '3.2 The value is printed on the LCD screen
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "Ki = " ; Ki
   Waitms 1000
   '3.3 Once done, the keyword R0ok is sent to LabView
   Print "Kiok"
   '4. Read the Kd value
   '--------------------
   '4.1 Once LabView is ready, the box receive the value for the Kd parameter
   Input Buffer
   Kd = Val(buffer)
   '4.2 The value is printed on the LCD screen
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "Kd = " ; Kd
   Waitms 1000
   '4.3 Once done, the keyword R0ok is sent to LabView
   Print "Kdok"
   '5. Read the Timeend value
   '-------------------------
   '5.1 Once LaBiew is ready, the box receive the value for the time end
   Input Buffer
   Timeend = Val(buffer)
   '5.2 The value is printed on the LCD screen
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "Time end = " ; Timeend
   Waitms 1000
   '5.3 Once done, the keyword R0ok is sent to LabView
   Print "Timeok"
   '6. Read number of element for the pressure law
   '----------------------------------------------
   '6.1 Once LaBiew is ready, the box receive the value for the time end
   Input Buffer
   Dimensionlaw = Val(buffer)
   '6.2 The value is printed on the LCD screen
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "Dim. law = " ; Dimensionlaw
   Waitms 1000
   '6.3 Once done, the keyword R0ok is sent to LabView
   Print "Dimok"
   '7. Read the gain for the pressure sensor
   '---------------------------------------------
   '7.1 Once LaBiew is ready, the box receive the value for gain
   Input Buffer
   Gainpressure = Val(buffer)
   '7.2 The value is printed on the LCD screen
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "Gain = " ; Gainpressure
   Waitms 1000
   '7.3 Once done, the keyword GainOK is sent to LabView
   Print "Gainok"
   '8. Read number of cycles
   '---------------------------------------------
   '8.1 Once LabView is ready, the box receive the value for number of cycle
   Input Buffer
   Numbercycle = Val(buffer)
   '8.2 The value is printed on the LCD screen
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "Number of cycles = " ; Numbercycle
   Waitms 1000
   '8.3 Once done, the keyword GainOK is sent to LabView
   Print "Cycleok"
   '9. Initilize the importation of data
   '------------------------------------
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "Ready to start"
   J = 0
   Timer2 = 0
   Stop Timer2
   Wait 1
   Goto Importdata                                          'Start the importion of the pressure law
Goto Main

' ------------------------------------------------------------------------------
' ----[B. Upload the arrays]----------------------------------------------------
' ------------------------------------------------------------------------------
Importdata:
   '1 Time
   '------
   '1.1 Print the state on the LCD screen
   Cls
   Cursor Off Noblink
   Lcd "Import time..."
   '1.2 Tell LabView the box is ready to receive the time vector
   Print "Time"
   '1.3 Begin the transfer
   For N = 1 To Dimensionlaw
       Input Buffer
       Arraytime(n) = Val(buffer)
   Next

   '2. Pressure
   '-----------
   '2.1 Print the stat on the LCD screen
   Cls
   Cursor Off Noblink
   Lcd "Import pressure..."
   '2.2 Tell LabView the box is ready to receive the pressure vector
   Print "Pressure"
   '2.3 Begin the transfer
   For N = 1 To Dimensionlaw
       Input Buffer
       Arraypressure(n) = Val(buffer)
   Next

   '3. End
   '------
   Cls
   Cursor Off Noblink
   Lcd "Importation done!"
Goto Starttimer                                             'Start the control sequence

' ------------------------------------------------------------------------------
' ----[C. Interpolate the pressure]---------------------------------------------
' ------------------------------------------------------------------------------
Starttimer:
   '1. Write the state on the LCD screen
   '------------------------------------
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "Imporation done"
   Locate 2 , 1 : Lcd "Start timer"
   Waitms 1000
   '2. Wait until LabView has flushed the buffer
   '--------------------------------------------
   Buffer = Waitkey()
   '3. Tell LabView the box is ready to begin the control sequence
   '--------------------------------------------------------------
   Print "Control"
   Buffer = Waitkey()
   '4. Once Labview is ready, start the timer which sample the control sequence
   '---------------------------------------------------------------------------
   Cls
   Cursor Off Noblink
   Locate 1 , 1 : Lcd "********************"
   Locate 2 , 1 : Lcd "* Control sequence *"
   Locate 3 , 1 : Lcd "*                  *"
   Locate 4 , 1 : Lcd "********************"
   Start Timer2
   Gosub Controlsequence
Return

Controlsequence:
   If Tim2isrbit = 1 Then
      Gosub Controller
      'Print Stopbyte
      'Cls
      'Locate 1 , 1 : Lcd Druck
      Reset Tim2isrbit
   End If
Goto Controlsequence

' ------------------------------------------------------------------------------
' ----[D. Timer increment]------------------------------------------------------
' ------------------------------------------------------------------------------
Tim2_isr:
   Set Tim2isrbit
   Timer2 = Timerstart
   'Load Timer2 , Timer2reload
Return

' ------------------------------------------------------------------------------
' ----[E. Write the functions]--------------------------------------------------
' ------------------------------------------------------------------------------
Function Pinterpolated(timein As Single) As Single
   Local A As Single
   Local B As Single
   Local Temp As Single
   Local Interpres As Single
   Local I As Integer
   Local Itemp As Integer
   Local Pressureindex As Integer
   '1. Look in the time array, for the index of the wanted time
   '-----------------------------------------------------------
   I = 1
   Itemp = Dimensionlaw + 1
   Do
      If Arraytime(i) > Timein Then
         Pressureindex = I - 1
         I = Dimensionlaw + 10
      End If
      Incr I
   Loop Until I > Itemp
   I = 1
   '2. Compute the interpolation
   '----------------------------
   ' Interpolation schema:
   ' p(t) = a * t + b
   ' with:
   '
   '  t_i < t < t_i+1
   '
   '      p(t_i) - p(t_i+1)
   '  a = -----------------
   '        t_i - t_i+1
   '
   '      t_i * p(t_i+1) - t_i+1 * p(t_i)
   '  b = -------------------------------
   '               t_i - t_i+1
   '
   A = Arraypressure(pressureindex) - Arraypressure(pressureindex + 1)
   Temp = Arraytime(pressureindex) - Arraytime(pressureindex + 1)
   A = A / Temp                                             'a
   '
   B = Arraytime(pressureindex) * Arraypressure(pressureindex + 1)
   Temp = Arraytime(pressureindex + 1) * Arraypressure(pressureindex)
   B = B - Temp
   Temp = Arraytime(pressureindex) - Arraytime(pressureindex + 1)
   B = B / Temp                                             'b
   '
   Interpres = A * Timein
   Interpres = Interpres + B                                'p(t) = a * t + b
   '
   Pinterpolated = Interpres
End Function

' ------------------------------------------------------------------------------
' ----[F. Get the pressure from the sensor]-------------------------------------
' ------------------------------------------------------------------------------
Get_druck:
   Druckbuffer = 0
   For N = 1 To 3
      Druckbuffer = Druckbuffer + Getadc(7)
   Next N
   Druckbuffer = Druckbuffer / 3                            'Take the average of 3 samples
   Druck = Druckbuffer
   Druck = Druck * Gainpressure                             'Pressure in [mbar]
Return

' ------------------------------------------------------------------------------
' ----[G. PID controller]-------------------------------------------------------
' ------------------------------------------------------------------------------
Controller:
   '1. Get the actual pressure in the system
   '----------------------------------------
   Gosub Get_druck

   '2. Increase the time:
   '---------------------
   T = T + Intervaltime

   '3. Compute the control values:
   '------------------------------
   '3.1 Scheduled pressure r(t):
   R = Pinterpolated(t)
   '3.2 Actual pressure y(t):
   Y = Druck
   '3.3 Error signal e(t-2), e(t-1) and e(t)
   E3 = E2                                                  'e(t-2)
   E2 = E1                                                  'e(t-1)
   E1 = R - Y                                               'e(t)

   '4. PID controller:
   '------------------
   'This is the discretization of the PID differential equation:
   ' du(t)         de(t)    1                 d^2e(t)
   ' ----- = K * ( ----- + ---- * e(t) + Td * ------- )
   '  dt            dt      Ti                  dt^2
   '
   'u(k) =
   '                              Kd                       Kd             Kd
   'u(k-1) + e(k)*(Kp + Ki * T + ----) - e(k-1)*(Kp + 2 * ----) + e(k-2)*----
   '                              T                         T              T
   '\---------------\/---------------/   \---------\/---------/  \----\/-----/
   '              Part 1                         Part 2              Part 3
   '4.1 Part 1:
   'Dim T1 As Single
   'Dim T2 As Single
   'Dim T3 As Single
   'Dim T4 As Single
   'Dim T5 As Single
   'T1 = Ki                                                  '* Timesampling
   'T2 = Kp + T1
   'T3 = Kd                                                  '/ Timesampling
   'T4 = T2 + T3
   'T5 = E1 * T4
   'Dim Ut1 As Single
   'Ut1 = U + T5
   ''4.2 Part 2
   'Dim T6 As Single
   'Dim T7 As Single
   'Dim T8 As Single
   'T6 = 2.0 * T3
   'T7 = Kp + T6
   'T8 = E2 * T7
   'Dim Ut2 As Single
   'Ut2 = Ut1 - T8
   ''4.3 Part 3
   'Dim T9 As Single
   'T9 = E3 * T3
   'Dim Ut3 As Single
   'Ut3 = Ut2 + T9
   'U = Ut1

   '4. PID controller (2nd algorithgm)
   '------------------------------------------
   'This is the discretisation of the equation:
   '														 de(t)
   'u(t) = Kp * e(t) + Ki * Integral( e(n), dn, 0, t) + Kd * -----
   '														   dt
   '
   'u(k) = P(k) + I(k) + D(k)
   '
   'P(k) = Kp * e(k)
   '
   'I(k) = Ki * dT * e(k) + Ki * dT * e(k-1)
   '
   'D(k) = Kd * (e(k) - e(k-1))/dT
   '
   '4.1 P(k)
   Dim Pk As Single
   Pk = Kp * E1
   '4.2 I(k)
   Dim Ik As Single
   Dim T1 As Single
   Dim T2 As Single
   T1 = Timesampling * E1
   T2 = T1 * Ki
   T1 = Timesampling * E2
   Sumik = T1 * Ki
   Ik = T2 + Sumik
   '4.3 D(k)
   Dim Dk As Single
   Dim T3 As Single
   Dim T4 As Single
   T3 = E1 - E2
   T4 = T3 / Timesampling
   Dk = Kd * T4
   '4.4 U(k)
   Dim T5 As Single
   T5 = Ik + Dk
   U = Pk + T5


   '5. Determine the direction of the motor:
   '----------------------------------------
   If U > 0 Then
      Direction = 1
   Elseif U < 0 Then
      Direction = 0
   End If

   '6. Take the absolute value of U and limit it [-255,255]
   '-------------------------------------------------------
   U = Abs(u)
   If U > 255 Then
      U = 255
   End If

   '7. Set the direction and motor speed
   '---------------------------------
   Ocr0 = Round(u)
   Porta.3 = Direction

   '8. Send the actual pressure and time to LabView
   '-----------------------------------------------
   'Print "Pressure" ; Y ; "Time" ; T

   '9. Stop condition
   '-----------------
   If T > Timeend Then
      '9.1 Increase the cycle counter
      Cyclecounter = Cyclecounter + 1
      '9.2 Reset the programm if all the cycles are done:
      If Cyclecounter = Numbercycle Then
         '9.2.1 Stop the timer (stop the control)
         Stop Timer2
         '9.2.2 Stop the motor
         Ocr0 = 0
         Porta.3 = 0
         '9.2.3 Send a stop message to labview: stop the import of the data
         Print "Stop"
         '9.2.4 Print the state on the LCD screend
         Cls
         Cursor Off Noblink
         Locate 1 , 1 : Lcd "Timer stopped"
         'Set Stopbyte
         'Print Stopbyte
         '9.2.5 Start the watchdog, to reset the program after 16ms
         Start Watchdog
         Waitms 200
      Else
         '9.2.6 If all the cycles are not done, set time to 0 and keep going!
         T = 0
      End If
   End If
Return