View previous topic :: View next topic |
Author |
Message |
EDC
Joined: 26 Mar 2014 Posts: 971
|
Posted: Fri Dec 27, 2019 5:03 pm Post subject: MPU6050 gyroscope and Rainbowleds means Level |
|
|
Hello. This is the code for rainbowleds stripe and MPU6050 gyroscope.
Can be used for some basic leveling or better for joy on some teeter/swing
Its hard to record this leds on the full brightness with the phone camera. Until now, when Im posting, I've remembered that for recording I always dimm these leds.
If sensor is "leveled" then in the middle of the stripe red dot is surrounded by two green dots.
If sensor is moved from the level (here scaled into the max +/-90 degres) then three leds move on the stripe an changing color from green to the red showing "how error is big"
I add digital filter for smoothness. Number of leds should be automatically calculated and adapted to your stripe len.
Maybe one will use only gyroscope part of the code.
Code: | $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 < 4 Then '50ms
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 |
|
|
Back to top |
|
|
albertsm
Joined: 09 Apr 2004 Posts: 5913 Location: Holland
|
Posted: Sat Dec 28, 2019 1:41 pm Post subject: |
|
|
hi EDC
Thank you for sharing, nice and interesting code
Would love to see a video. But i can imagine how it works. _________________ Mark |
|
Back to top |
|
|
Meister
Joined: 27 May 2010 Posts: 319
|
Posted: Fri Jan 31, 2020 9:13 am Post subject: |
|
|
EDC,
thanks for posting your code.
I had built that project to (hopefully) enjoy and teach a little my 7-year-old grandson tomorrow.
Works nicely.
Best regards, Meister |
|
Back to top |
|
|
EDC
Joined: 26 Mar 2014 Posts: 971
|
Posted: Fri Jan 31, 2020 9:35 am Post subject: |
|
|
Its nice that this gets some feedback
The secret is that this is a ARCADE GAME FOR PEOPLE ON THE WEDDING
They have hard to beat the slalom with this level in the hand when they are drunk ahahahaha
So here you have version that can be calibrated for the maximum angle. Maximum angle will be mapped for whole stripe so no need to 90degres to reach the stripe end.
Addictionally there is some flashing effect and whole stripe became red indicating that this try is fail
Maybe someone will be interested how map different angles to some values.
Have fun!
Code: | $regfile = "m328pdef.dat"
$crystal = 16000000
$hwstack = 64
$swstack = 16
$framesize = 64
$baud = 115200
'*************************************************************************************
'* by BARTek EDC 13/01/2020 Toy. MPU6050 with Rainbowleds stripe *
'* Stripe PC.0 *
'* Buzzer PC.1 *
'* Switch PC.2 *
'* *
'* Long press on the switch calibrate (sets max angle). If angle will be exceeded *
'* then level will block and whole stripe is red. Only after switch press the toy *
'* will be released again. *
'*************************************************************************************
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
'*************************************************
'* I/O CONFIG *
'*************************************************
Buzz Alias Portc.1 : Config Buzz = Output
Dim Odlicz_buzz As Byte : Set Buzz
Sw1 Alias Pinc.2 : Set Portc.2 : Config Sw1 = Input
Dim Sw_cnt As Byte , Sw_lock As Byte
'*************************************************
'* VARIABLES *
'*************************************************
'MPU6050
Const Mpu6050_write = &HD0
Const Mpu6050_read = &HD1
Dim Tempbuff(14) As Byte , Helpb As Byte , Tempb As Byte , N 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 Mpu_int As Integer , Average_int As Integer
Dim Divider As Integer , Diff As Integer , Diff2 As Integer
Dim Bar_locked As Byte
'digital filter for smooth move
Dim Suma As Dword , Helpd As Dword
Dim Led_half As Byte
Led_half = Number_of_leds / 2
'*************************************************
'* EEPROM *
'*************************************************
Const Ee_mark = 10
Dim Ee_check As Eram Byte , Divider_ee As Eram Integer
Dim Minimum As Integer , Minimum_ee As Eram Integer
Dim Maximum As Integer , Maximum_ee As Eram Integer
Dim Minmax As Integer , Minmax_ee As Eram Integer
Helpb = Ee_check
If Helpb <> Ee_mark Then
Ee_check = Ee_mark
Const Tempc = 30000 / Number_of_leds 'MPU605 values are trimmed in this code from -15000 to 15000 = range 30000
Divider_ee = Tempc
Minimum_ee = 0
Maximum_ee = 30000
Minmax_ee = 30000
End If
'odczyt z eeprom
Divider = Divider_ee
Minimum = Minimum_ee
Maximum = Maximum_ee
Minmax = Minmax_ee
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
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 Map2(byval Value As Word , Byval Min_in As Word , Byval Max_in As Word , Byval Min_out As Word , Byval Max_out As Word)as Word
Local I1 As Dword
Local I2 As Dword
I1 = Value - Min_in
I2 = Max_out - Min_out
I1 = I1 * I2
I2 = Max_in - Min_in
I1 = I1 / I2
I1 = I1 + Min_out
Map2 = I1 'Abs(i1)
If Map2 >= Max_out Then Map2 = 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
Sub Take_maxs()
If Average_int > 15000 Then '15000-30000 sens up, LED_MAX
Maximum = Average_int
Maximum_ee = Maximum
Diff = Maximum - 15000
Minimum = 15000 - Diff
Minimum_ee = Minimum
Else '0-15000 sens down, LED_MIN
Minimum = Average_int
Minimum_ee = Minimum
Diff = 15000 - Minimum
Maximum = 15000 + Diff
Maximum_ee = Maximum
End If
'Print "Min=" ; Minimum ; " Max=" ; Maximum
End Sub
Sub Blinking(byval Times As Byte)
For N = 1 To Times
Rb_clearcolors
Cg = 0 : Cr = 200
Rb_fillcolors Color1(_base)
Rb_send
Reset Buzz
Waitms 350
Rb_clearcolors
Rb_send
Set Buzz
Waitms 350
Next
End Sub
Call Blinking(3)
Do
If Tifr2.ocf2a = 1 Then '10ms
Tifr2.ocf2a = 1
'buzzer
'-----------------------------------
If Odlicz_buzz > 0 Then
Decr Odlicz_buzz
If Odlicz_buzz = 0 Then Set Buzz
End If
'switch
'-----------------------------------
If Sw1 = 0 Then
If Sw_lock = 0 Then
If Sw_cnt < 150 Then
Incr Sw_cnt
Else
Sw_lock = 1
Rb_clearcolors
Cg = 200 : Cr = 0
Rb_fillcolors Color1(_base)
Rb_send
Wait 1
Call Take_maxs()
End If
End If
Else
If Sw_lock = 0 Then
If Sw_cnt > 5 Then
Reset Buzz
Odlicz_buzz = 5
Bar_locked = 0
End If
End If
Sw_cnt = 0
Sw_lock = 0
End If
'-----------------------------------
If Miliseconds < 2 Then
Incr Miliseconds
Else
Miliseconds = 0
Call Read_mpu6050
Mpu_int = Dx 'X axis
'Mpu_int = Dy 'Y axis
'Mpu_int = Dz 'Z axis
Select Case Mpu_int
Case Is < -15000
Mpu_int = -15000
Case Is > 15000
Mpu_int = 15000
Case Else
nop
End Select
Myint = Mpu_int + 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
Average_int = Helpd ' (value 0-30000)
'-------------------------
Myint = Average_int
Select Case Myint
Case Is < Minimum
Myint = Minimum
Case Is > Maximum
Myint = Maximum
Case Else
nop
End Select
Tempi = Map2(myint , Minimum , Maximum , 0 , 30000) '
Tempi = Tempi / Divider
Mypos = Tempi
If Mypos > Zero_base_ledow Then Mypos = Zero_base_ledow
If Bar_locked = 0 Then
If Old_pos <> Mypos Then
Old_pos = Mypos
Print Minimum ; " ->" ; Maximum ; " ->" ; Divider ; " ->" ; Myint ; " ->" ; 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
'--------------------------------------
Select Case Mypos
Case 0
Bar_locked = 1
Call Blinking(4)
Rb_clearcolors
Cg = 0 : Cr = 200
Rb_fillcolors Color1(_base)
Reset Buzz
Odlicz_buzz = 100
Case Zero_base_ledow
Bar_locked = 1
Call Blinking(4)
Rb_clearcolors
Cg = 0 : Cr = 200
Rb_fillcolors Color1(_base)
Reset Buzz
Odlicz_buzz = 100
End Select
Rb_send
'if Old_pos<>
End If
'if Bar_locked
End If
'100ms
End If
'10ms
End If
Loop
End |
|
|
Back to top |
|
|
|
|
You cannot post new topics in this forum You cannot reply to topics in this forum You cannot edit your posts in this forum You cannot delete your posts in this forum You cannot vote in polls in this forum You cannot attach files in this forum You cannot download files in this forum
|
|