Poziomnica na Rainbowleds i MPU6050
: 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.
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