$regfile = "m328pdef.dat"
$crystal = 8000000
$hwstack = 100
$swstack = 100
$framesize = 150
$baud = 4800
$lib "I2C_TWI.LBX"
'----------------- Переменные-----------------------------------------------------------
Dim I As Byte
Dim Z As Byte
Dim N As Byte
Dim Ind_max As Byte
Dim Максимум As Byte
Dim Флаг_спать As Byte
Dim Флаг_запуска As Byte
Dim Можно_поспать As Byte
Dim Массив_гиро_х(255) As Byte


Dim Register(10) As Byte
Dim Acc_x As Integer At Register(1) Overlay
Dim Acc_y As Integer At Register(3) Overlay
Dim Acc_z As Integer At Register(5) Overlay
Dim T_raw As Integer At Register(7) Overlay
Dim Gyro_x As Integer At Register(9) Overlay


Dim Принятый_сигнал As Dword
Dim Данные_передачи As Dword
Dim Данные_ацп_avcc_word As Word :

Dim N_датчика As String * 1
Dim Состояние As String * 1
Dim Id_комплекта_string As String * 5
Dim Данные_ацп_avcc_string As String * 3
Dim Принятый_сигнал_string As String * 10
Dim Данные_передачи_string As String * 10
Dim Тревога As Integer
Dim Калман_временная As Integer
Dim Предел_измерения As Integer
Dim Коэффициент_диапазона As Integer
Dim Значение_текущее(4) As Single                 'результирующее значение                                        
                                                                        'исходное значение
Dim Значение_предыдущее(4) As Single              'результат вычисления в предыдущей интерации                                                                
                                                                             
N_датчика = "1"
Данные_ацп_avcc_word = 0
Id_комплекта_string = "10000"
Const K = 0.1 : Const K1 = 1 - K
'_--------------------------------------------------------------------------------------------------
Config Portd.6 = Output : 6050_pit Alias Portd.6
Config Portd.7 = Output : Hc12_pit Alias Portd.7
Config Scl = Portc.5
Config Sda = Portc.4
I2cinit
Config Adc = Single , Prescaler = 128 , Reference = Avcc

On Urxc Прием
On Utxc Передача_закончена
On Wdt Прерывание_watchdog Nosave
On Adc Обработка_прерывания_ацп Nosave



Enable Utxc
Enable Urxc
Enable Wdt


                                     ' Переход на подпрограмму обработки прерывания после переполнения таймера-счётчика Watchdog

Config Watchdog = 4096
Wdtcsr.wde = 0 
Wdtcsr.wdie = 1                                   'таймер настраиваем  в режим прерываний
Start Watchdog


Smcr = &B000_0101                                 'режим сна powerdown
Const W_addr = &HD0                               'Адреса чтения и записи
Const R_addr = &HD1
Enable Interrupts

'----------Идентификация и настройка сенсора ------------------------------------
'Сбрасываем гироскоп, акселерометр и датчик темпереатуры, нициализация I2C
I2cstart
I2cwbyte W_addr
I2cwbyte &H68
I2cwbyte &H07
I2cstop
Waitms 10
'Конфигурации питания и синхронизации(синхронизация по оси Х гироскопа , спящего режима нет)
I2cstart
I2cwbyte W_addr
I2cwbyte &H6B
I2cwbyte &H01
I2cstop
Waitms 10
'Конфигурации гироскопа(настроим на +-2000°ss )
I2cstart
I2cwbyte W_addr
I2cwbyte &H1B
I2cwbyte &H11
I2cstop
Waitms 10
Dim Z1 As Byte
Z1 = 1
'----При первом запуске настраиваем датчик--------------------------------------
If Флаг_запуска = 0 Then
   Состояние = "3"
   Gosub Измерение_avcc
   Gosub Передача                                 'при первом запуске
   '**************************************тут самонастройка
   Флаг_запуска = 1
End If
'================Тело программы=================================================
Do
  '++++++++++++++++++++часть кода для проверки++++++++++++++++++++++++++++++++++
   If N = 7 Then
      N = 0
      Incr Z
   End If
   If Z = 5 Then Z = 1
   Select Case Z
      Case 1 : Состояние = "1"
      Case 2 : Состояние = "2"
      Case 3 : Состояние = "4"
      Case 4 : Состояние = "3"
   End Select
   'Incr N
   Select Case N
      Case 1 : N_датчика = "1"
      Case 2 : N_датчика = "2"
      Case 3 : N_датчика = "3"
      Case 4 : N_датчика = "4"
      Case 5 : N_датчика = "5"
      Case 6 : N_датчика = "6"
   End Select
   '++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   Gosub Измерение_avcc
   Gosub Передача
   If Флаг_спать <> 0 Then
      Флаг_спать = 0
      Powerdown
   End If

Loop
Обнаружить_вращение:
'---------------------------------------------------------------------------
Тревога = Round(значение_текущее(4))
Тревога = Abs(тревога)
Массив_гиро_х(z) = Тревога
Max(Массив_гиро_х(1) , Максимум , Ind_max)
If Максимум > 40 Then
   Состояние = "1"
End If
If Максимум < 40 Then
   Состояние = "2"
End If
'---------------------------------------------------------------------------
Return
'-------Фильтр Кальмана---------------------------------------------------
Фильтр_калмана:
'---------------------------------------------------------------------------
For N = 1 To 4
   Select Case Z
      Case 1:
         Калман_временная = Acc_x
         Коэффициент_диапазона = 2
      Case 2:
         Калман_временная = Acc_y
         Коэффициент_диапазона = 2
      Case 3:
         Калман_временная = Acc_z
         Коэффициент_диапазона = 2
      Case 4:
         Калман_временная = Gyro_x
         Коэффициент_диапазона = 2000
   End Select
   Значение_текущее(n) = K * Калман_временная
   Значение_предыдущее(n) = Значение_предыдущее(n) * K1
   Значение_текущее(n) = Значение_текущее(n) + Значение_предыдущее(n)
   Значение_предыдущее(n) = Значение_текущее(n)
   Значение_текущее(n) = Значение_текущее(n) / 32768
   Значение_текущее(n) = Значение_текущее(n) * Коэффициент_диапазона
Next
'---------------------------------------------------------------------------
Return

Измерение_avcc:
'--------------------------------------------------------------------------
   Enable Adc
   Adcsra.aden = 1
   Waitms 250
   Admux = &B_0100_1111
   Waitus 70
   !sts MCUCR , R16
   !sleep
   !nop
   Admux = &B_0100_1110
   Waitus 70
   !ldi R16 , &B_1001_0000
   !sts MCUCR , R16
   !sleep
   !nop
   Disable Adc
   Adcsra.aden = 0
   Acsr.acd = 0
'---------------------------------------------------------------------------
Return

Передача:
'--------------------------------------------------------------------------
Disable Urxc
Данные_ацп_avcc_string = Str(данные_ацп_avcc_word)
Данные_передачи_string = Id_комплекта_string + N_датчика + Состояние + Данные_ацп_avcc_string
Данные_передачи = Val(данные_передачи_string)
Set 6050_pit
Set Hc12_pit : Waitms 100
Printbin Данные_передачи
If Флаг_запуска = 1 Then
   Waitms 600
   Reset Hc12_pit
   Reset 6050_pit
End If
Enable Urxc
'---------------------------------------------------------------------------
Return

Обработка_прерывания_ацп:
'---------------------------------------------------------------------------
!push R16
!lds R16 , adcl
!sts {Данные_ацп_avcc_word} , R16
!lds R16 , adch
!sts {Данные_ацп_avcc_word + 1} , R16
!pop R16
!reti
'---------------------------------------------------------------------------
Return

Прием:
'----------------------------------------------------------------------------
Inputbin Принятый_сигнал
Return

Прерывание_watchdog:
'----------------------------------------------------------------------------
   Incr N
Return



Передача_закончена:
'-------------------------------------------------------------------------------
$asm
   push R16
   ldi R16 , 1
   sts {Флаг_спать} , R16
   pop R16
   reti
$end Asm
'-------------------------------------------------------------------------------
Return