Poziomnica na Rainbowleds i MPU6050

niveasoft · Post autor: **niveasoft** » 27 gru 2019, 18:06

Tak kodzik który taśmę Rainbowled`ów i żyroskop zamienia w poziomnicę.
Po małej przeróbce (bo teraz wyskalowane na +/-90stopni) można sobie przykręcić do huśtawki

Kiedy czujnik jest w poziomie to led na samym srodku taśmy swieci na czerwono i jest otoczony dwoma zielonymi

Kiedy czujnik ma inny kąt to trzy ledy przemieszczają się po taśmie znieniając kolor na czerwony tym bardziej im większy kąt.

Code: [Zaznacz cały] [Rozwiń/Zwiń]

$regfile = "m328pdef.dat"
$crystal = 16000000
$hwstack = 64
$swstack = 16
$framesize = 64
$baud = 115200
Const Number_of_leds = 45
Const Pwm_8bit = 32 '0-254
Const Pwm_proc = 15 '0-100
Config Submode = New 'first write subs then use them
'*************************************************
'* I2C/TWI CONFIG FOR MPU6050 *
'*************************************************
$lib "i2c_twi.lib" ' AVR hardware TWI (I2C)
Config Scl = Portc.5
Config Sda = Portc.4
I2cinit
Config Twi = 400000
'*************************************************
'* TIMER2 CONFIG FOR TIME BASE *
'*************************************************
Config Timer2 = Timer , Prescale = 1024 , Clear_timer = 1
Compare2a = 155 '10ms @16MHz/1024
Dim Miliseconds As Byte
'*************************************************
'* RAINBOWLEDS *
'*************************************************
Config Rainbow = 1 , Rb0_len = Number_of_leds , Rb0_port = Portc , Rb0_pin = 0
' ^ connected to pin 0
' ^------------ connected to portB
' ^-------------------------- 8 leds on stripe
' ^------------------------------------- 1 channel
'Global Color-variables
Dim Color1(3) As Byte
Cr Alias Color1(_base) : Cg Alias Color1(_base + 1) : Cb Alias Color1(_base + 2)
Dim Color2(3) As Byte
Cr2 Alias Color2(_base) : Cg2 Alias Color2(_base + 1) : Cb2 Alias Color2(_base + 2)
Const Zero_base_ledow = Number_of_leds - 1
'*************************************************
'* VARIABLES *
'*************************************************
'MPU6050
Const Mpu6050_write = &HD0
Const Mpu6050_read = &HD1
Dim Tempbuff(14) As Byte , Helpb As Byte , Tempb As Byte
Dim Rawbuff(14) As Byte
Dim Dx As Integer At Rawbuff(1) Overlay
Dim Dy As Integer At Rawbuff(3) Overlay
Dim Dz As Integer At Rawbuff(5) Overlay
Dim Th As Integer At Rawbuff(7) Overlay
Dim Gx As Integer At Rawbuff(9) Overlay
Dim Gy As Integer At Rawbuff(11) Overlay
Dim Gz As Integer At Rawbuff(13) Overlay
Dim Mypos As Byte , Old_pos As Byte , Heat As Byte
Dim Myint As Integer , Tempi As Integer
Dim Divider As Integer
'digital filter for smooth move
Dim Suma As Dword , Helpd As Dword
Dim Led_half As Byte
Led_half = Number_of_leds / 2
Sub Init_mpu
'--- (25 ) Sample Rate Divider = 1 ---
I2cstart
I2cwbyte Mpu6050_write
I2cwbyte 25
I2cwbyte &B00000000
I2cstop
'--- (26 ) DLPF = 42/44 Hz ---
I2cstart
I2cwbyte Mpu6050_write
I2cwbyte 26
I2cwbyte &B00000011
I2cstop
'--- (27 ) Gyro Full Range = +-2000Â°/s ---
I2cstart
I2cwbyte Mpu6050_write
I2cwbyte 27
I2cwbyte &B00011000
I2cstop
'--- (28 ) ACC Full Range = +-2g ---
I2cstart
I2cwbyte Mpu6050_write
I2cwbyte 28
I2cwbyte &B00000000
I2cstop
'--- (107) Power Management 1 ---
I2cstart
I2cwbyte Mpu6050_write
I2cwbyte 107
I2cwbyte &B00001011
I2cstop
End Sub
Call Init_mpu
Sub Read_mpu6050
Tempbuff(1) = 59 'address of first byte to read
I2creceive Mpu6050_read , Tempbuff(1) , 1 , 14
Helpb = Memcopy(tempbuff(1) , Rawbuff(1) , 14)
Swap Dx : Swap Dy : Swap Dz : Swap Th : Swap Gx : Swap Gy : Swap Gz
End Sub
Waitms 50
Rb_selectchannel 0 ' select first channel
Rb_clearcolors
Rb_send
Cr = 75 : Cg = 25 : Cb = 0
Divider = 30000 / Number_of_leds 'MPU605 values are trimmed in this code from -15000 to 15000 = range 30000
Function Map(byval Value As Byte , Byval Min_in As Byte , Byval Max_in As Byte , Byval Min_out As Byte , Byval Max_out As Byte)as Byte
Local I1 As Integer
Local I2 As Integer
I1 = Value - Min_in
I2 = Max_out - Min_out
I1 = I1 * I2
I2 = Max_in - Min_in
I1 = I1 / I2
I1 = I1 + Min_out
Map = Abs(i1)
If Map >= Max_out Then Map = Max_out - 1
End Function
Function Bright_8bit(byval Value As Byte , Byval 8bit As Byte)as Byte
Local W1 As Word
W1 = Value * 8bit
Shift W1 , Right , 8
Bright_8bit = W1
End Function
Function Bright_procent(byval Value As Byte , Byval Procents As Byte)as Byte
Local W2 As Word
W2 = Value * Procents
W2 = W2 / 100
Bright_procent = W2
End Function
Do
If Tifr2.ocf2a = 1 Then '10ms
Tifr2.ocf2a = 1
If Miliseconds < 2 Then
Incr Miliseconds
Else
Miliseconds = 0
Call Read_mpu6050
Myint = Dx 'X axis
'Myint = Dy 'Y axis
'Myint = Dz 'Z axis
If Myint < -15000 Then Myint = -15000
If Myint > 15000 Then Myint = 15000
Myint = Myint + 15000 'offset for unsigned value 0-30000
'digital filter for smoothness
'-------------------------
Helpd = Suma
Shift Helpd , Right , 3
Suma = Suma - Helpd
Suma = Suma + Myint
Helpd = Suma
Shift Helpd , Right , 3
Myint = Helpd
'-------------------------
Tempi = Myint / Divider
Mypos = Tempi '
'Print Myint ; " ->" ; Mypos
If Mypos > Zero_base_ledow Then Mypos = Zero_base_ledow
If Old_pos <> Mypos Then
Old_pos = Mypos
Select Case Mypos
Case Is < Led_half
Heat = Led_half - Mypos
Case Led_half
Heat = 0
Case Is > Led_half
Heat = Mypos - Led_half
End Select
Rb_clearcolors 'always clear
'--------------------[MAIN DOT1]------
Tempb = Map(heat , 0 , Led_half , 0 , 254)
'Print Heat ; " -> " ; Tempb
Cr = Tempb
Cg = 254 - Tempb
If Heat <> 0 Then 'only if not centered
Rb_setcolor Mypos , Color1(_base)
End If
'---------[DIMMING DOTS ON THE SIDES]-
Cr2 = Bright_8bit(cr , Pwm_8bit)
Cg2 = Bright_8bit(cg , Pwm_8bit)
'Cr2 = Bright_procent(cr , Pwm_proc)
'Cg2 = Bright_procent(cg , Pwm_proc)
'--------------------[BOTTOM DOT]------
If Mypos > 0 Then
Helpb = Mypos - 1
Rb_setcolor Helpb , Color2(_base)
End If
'--------------------[TOP DOT]---------
If Mypos < Zero_base_ledow Then
Helpb = Mypos + 1
Rb_setcolor Helpb , Color2(_base)
End If
'--------------------[MAIN DOT2]-------'
If Heat = 0 Then 'if centered then effect "red dot between greens"
Cr = 254 : Cg = 0
Rb_setcolor Mypos , Color1(_base)
End If
'--------------------------------------
Rb_send
End If
'100ms
End If
'10ms
End If
Loop
End