Code:
$regfile = "xm32a4def.dat"
$framesize = 100
$swstack = 100
$hwstack = 100
$crystal = 32000000
$lib "xmega.lib"
$external _xmegafix_clear
$external _xmegafix_rol_r1014
Config Osc = Disabled , 32mhzosc = Enabled
Config Sysclock = 32mhz , Prescalea = 1 , Prescalebc = 1_1
Declare Sub Rcsignal()
Declare Sub Pidregulator()
Declare Sub Drivemotors()
Declare Sub Init_mpu()
Declare Sub Read_gyro()
Declare Sub Read_acc()
'--RC Settings--
Config Pinc.2 = Input
On Portc_int0 Getreceiver
Enable Portc_int0 , Lo
Portc_pin2ctrl = &B00_011_001
Portc_int0mask = &B0000_0100
Portc_intctrl = &B0000_00_01
Config Tcc0 = Normal , Prescale = 256 , Resolution = 16
On Tcc0_ovf Detectrxpause
Enable Tcc0_ovf , Lo
'--UART Settings--
Config Com2 = 38400 , Mode = Asynchroneous , Parity = None , Stopbits = 1 , Databits = 8
Config Serialin1 = Buffered , Size = 254
Config Serialout1 = Buffered , Size = 254
Open "COM2:" For Binary As #1
'--TWI Settings--
Dim Twi_start As Byte
Open "twie" For Binary As #2
Config Twie = 400000
I2cinit #2
'--Motor Constants--
Const Pwm_mot_off = 20000
Const Pwm_mot_max = 60000
'--PWM Settings--
Config Tcd0 = Pwm , Prescale = 1 , Comparea = Enabled , Compareb = Enabled , Comparec = Enabled , Compared = Enabled , Event_source = Off , Event_action = Off , Event_delay = Disabled , Resolution = 16
Tcd0_cca = Pwm_mot_off
Tcd0_ccb = Pwm_mot_off
Tcd0_ccc = Pwm_mot_off
Tcd0_ccd = Pwm_mot_off
'--LED--
Config Portb.0 = Output
Config Portb.1 = Output
Config Portb.2 = Output
Led1 Alias Portb.0
Led2 Alias Portb.1
Led3 Alias Portb.2
Led1 = 0
Led2 = 0
Led3 = 0
'--MPU 60X0--
Const Mpuaddw = &B11010000
Const Mpuaddr = &B11010001
Dim Gyrox As Integer
Dim Gyroy As Integer
Dim Gyroz As Integer
Dim Tmp_gyrox(2) As Byte At Gyrox Overlay
Dim Tmp_gyroy(2) As Byte At Gyroy Overlay
Dim Tmp_gyroz(2) As Byte At Gyroz Overlay
Dim Accx As Integer
Dim Accy As Integer
Dim Tmp_accx(2) As Byte At Accx Overlay
Dim Tmp_accy(2) As Byte At Accy Overlay
Dim Gyrox_offset As Integer
Dim Gyroy_offset As Integer
Dim Gyroz_offset As Integer
Dim Accx_offset As Integer
Dim Accy_offset As Integer
Dim Roll_check(50) As Integer
Dim Nick_check(50) As Integer
Dim Yaw_check(50) As Integer
Dim Check_tmp As Integer
Dim Offset_tmp As Long
Dim Movement As Byte
Dim Compare_tmp As Integer
'--RC Channel--
Const Throttlechannel = 1
Const Nickchannel = 3
Const Rollchannel = 2
Const Yawchannel = 4
Const Statechannel = 5
'--Read Receiver (Rx)--
Const Maxrcchannel = 8
Dim Empftmp(maxrcchannel) As Word
Dim Empf(maxrcchannel) As Word
Dim Channel As Byte
Dim Lpempf(maxrcchannel) As Word
Dim Intempf(maxrcchannel) As Integer
Dim Receiver_check As Byte
Dim Rc_set_throttle As Long
Dim Rc_set_roll As Long
Dim Rc_set_nick As Long
Dim Rc_set_yaw As Long
Dim Acro_sensitivy As Word
Dim Hover_sensitivy As Word
Dim Yaw_sensitivy As Word
'--Motors--
Dim Vorwahl_pwm As Word
Dim Vorwahl As Word
Dim Motor_rear_left As Long
Dim Motor_rear_right As Long
Dim Motor_front_right As Long
Dim Motor_front_left As Long
'--Control Loop (PID)--
Dim Acro_p As Word
Dim Acro_i As Word
Dim Acro_d As Word
Dim Hover_p As Word
Dim Hover_i As Word
Dim Hover_d As Word
Dim Yaw_p As Word
Dim Yaw_i As Word
Dim Xangle As Single
Dim Yangle As Single
Dim Gyro_yangle As Single
Dim Gyro_xangle As Single
Dim Gyro_influence As Single
Dim Acc_influence As Single
Dim Roll_error As Long
Dim Nick_error As Long
Dim Yaw_error As Long
Dim Roll_error_integral As Long
Dim Nick_error_integral As Long
Dim Yaw_error_integral As Long
Dim Roll_error_differential As Long
Dim Nick_error_differential As Long
Dim Roll_error_old As Long
Dim Nick_error_old As Long
Dim P_set_roll As Long
Dim I_set_roll As Long
Dim D_set_roll As Long
Dim P_set_nick As Long
Dim I_set_nick As Long
Dim D_set_nick As Long
Dim P_set_yaw As Long
Dim I_set_yaw As Long
'-- Internal Variables --
Dim Failsave As Byte
Dim State As Byte
Dim I As Byte
'--Startup--
Vorwahl = 5000
Failsave = 0
'8 Zoll Props:
Acro_p = 270 'Acro_p = 270 / 300
Acro_i = 62 'Acro_i = 62 / 70
Acro_d = 20 'Acro_d = 20 / 25
Acro_sensitivy = 320
Yaw_p = 120 '160
Yaw_i = 29 '40
Hover_p = 40
Hover_i = 25
Hover_d = 140
State = 0
Hover_sensitivy = 100
Yaw_sensitivy = 400
Gyro_influence = 0.99
Acc_influence = 1 - Gyro_influence
Lpempf(1) = 24
Lpempf(2) = 100
Lpempf(3) = 100
Lpempf(4) = 100
Lpempf(5) = 24
Lpempf(6) = 24
Lpempf(7) = 24
Lpempf(8) = 24
Waitms 250
Call Init_mpu()
Waitms 250
Clear Serialin1
Clear Serialout1
'--Get Offset--
For I = 1 To 10
Toggle Led1
Waitms 100
Next I
Led1 = 0
Gyrox_offset = 0
Gyroy_offset = 0
Gyroz_offset = 0
Movement = 1
While Movement = 1
Movement = 0
For I = 1 To 50
Call Read_gyro()
Roll_check(i) = Gyrox
Nick_check(i) = Gyroy
Yaw_check(i) = Gyroz
Waitms 10
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Roll_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Roll_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= 64 Then Movement = 1
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Nick_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Nick_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= 64 Then Movement = 1
Next I
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Yaw_check(i)
Next I
Offset_tmp = Offset_tmp / 50
For I = 1 To 50
Compare_tmp = Offset_tmp - Yaw_check(i)
Compare_tmp = Abs(compare_tmp)
If Compare_tmp >= 64 Then Movement = 1
Next I
Toggle Led1
Wend
Led1 = 0
For I = 1 To 10
Toggle Led2
Waitms 100
Next I
Led2 = 0
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Roll_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyrox_offset = Offset_tmp
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Nick_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyroy_offset = Offset_tmp
Offset_tmp = 0
For I = 1 To 50
Offset_tmp = Offset_tmp + Yaw_check(i)
Next I
Offset_tmp = Offset_tmp / 50
Gyroz_offset = Offset_tmp
'--Enable Interrupts--
Config Priority = Static , Vector = Application , Lo = Enabled , Med = Enabled , Hi = Enabled
Enable Interrupts
Waitms 100
'check for receiver
Receiver_check = 0
While Receiver_check < 200
If Empf(statechannel) > 50 Or Empf(statechannel) < 20 Or Empf(throttlechannel) > 50 Or Empf(throttlechannel) < 20 Then
If Receiver_check > 0 Then Decr Receiver_check
Else
If Receiver_check < 250 Then Incr Receiver_check
End If
Wend
Do
Call Rcsignal()
Call Pidregulator()
Call Drivemotors()
Loop
End
Sub Rcsignal()
For I = 1 To 4
Lpempf(i) = Lpempf(i) * 3
Lpempf(i) = Lpempf(i) + Empf(i)
Shift Lpempf(i) , Right , 2 , Signed
If Lpempf(i) < 22 Then Lpempf(i) = 22
If Lpempf(i) >= 179 Then Lpempf(i) = 178
Next I
For I = 5 To Maxrcchannel
Lpempf(i) = Lpempf(i) * 7
Lpempf(i) = Lpempf(i) + Empf(i)
Shift Lpempf(i) , Right , 3 , Signed
If Lpempf(i) < 22 Then Lpempf(i) = 22
If Lpempf(i) >= 179 Then Lpempf(i) = 178
Next I
Intempf(1) = Lpempf(throttlechannel) - 22
For I = 2 To Maxrcchannel
Intempf(i) = Lpempf(i) - 100
Next I
If Channel >= 12 Then
If Failsave < 250 Then Incr Failsave
Else
If Failsave >= 1 Then Decr Failsave
End If
If State = 0 Then
If Intempf(throttlechannel) < 20 Then
If Intempf(statechannel) >= -30 And Intempf(statechannel) < 30 Then
State = 1
End If
End If
Else
If Intempf(statechannel) < -30 Then
If Intempf(throttlechannel) < 20 Then
State = 0
End If
Elseif Intempf(statechannel) >= -30 And Intempf(statechannel) < 30 Then
State = 1
Elseif Intempf(statechannel) >= 30 Then
State = 2
End If
End If
Led1 = 0
Led2 = 0
Led3 = 0
If State = 0 Then Led1 = 1
If State = 1 Then Led2 = 1
If State = 2 Then Led3 = 1
Rc_set_throttle = Intempf(throttlechannel) * 250
Rc_set_yaw = Intempf(yawchannel) * Yaw_sensitivy
End Sub
Sub Pidregulator()
Call Read_gyro()
If State = 1 Then
Rc_set_roll = Intempf(rollchannel) * Acro_sensitivy
Rc_set_nick = Intempf(nickchannel) * Acro_sensitivy
Roll_error = Gyrox
Roll_error = Roll_error - Rc_set_roll
Roll_error_integral = Roll_error_integral + Roll_error
If Roll_error_integral >= 320000 Then Roll_error_integral = 320000
If Roll_error_integral < -320000 Then Roll_error_integral = -320000
Roll_error_differential = Roll_error - Roll_error_old
Roll_error_old = Roll_error
P_set_roll = Roll_error * Acro_p
Shift P_set_roll , Right , 9 , Signed 'P_set_roll = P_set_roll / 512
I_set_roll = Roll_error_integral * Acro_i
Shift I_set_roll , Right , 14 , Signed 'I_set_roll = I_set_roll / 16384
D_set_roll = Roll_error_differential * Acro_d
Shift D_set_roll , Right , 9 , Signed 'D_set_roll = D_set_roll / 512
Nick_error = Gyroy
Nick_error = Nick_error - Rc_set_nick
Nick_error_integral = Nick_error_integral + Nick_error
If Nick_error_integral >= 320000 Then Nick_error_integral = 320000
If Nick_error_integral < -320000 Then Nick_error_integral = -320000
Nick_error_differential = Nick_error - Nick_error_old
Nick_error_old = Nick_error
P_set_nick = Nick_error * Acro_p
Shift P_set_nick , Right , 9 , Signed 'P_set_nick = P_set_nick / 512
I_set_nick = Nick_error_integral * Acro_i
Shift I_set_nick , Right , 14 , Signed 'I_set_nick = I_set_Nick / 16384
D_set_nick = Nick_error_differential * Acro_d
Shift D_set_nick , Right , 9 , Signed 'D_set_nick = D_set_nick / 512
Elseif State = 2 Then
Call Read_acc()
Rc_set_roll = Intempf(rollchannel) * Hover_sensitivy
Rc_set_nick = Intempf(nickchannel) * Hover_sensitivy
Gyro_yangle = Gyro_yangle + Gyrox
Gyro_yangle = Gyro_yangle * Gyro_influence
Yangle = Accy * Acc_influence
Gyro_yangle = Gyro_yangle + Yangle
Roll_error = Gyro_yangle - Rc_set_roll
Roll_error_integral = Roll_error_integral + Roll_error
If Roll_error_integral >= 4000000 Then Roll_error_integral = 4000000
If Roll_error_integral < -4000000 Then Roll_error_integral = -4000000
P_set_roll = Roll_error * Hover_p
Shift P_set_roll , Right , 8 , Signed 'P_set_roll = P_set_roll / 256
I_set_roll = Roll_error_integral * Hover_i
Shift I_set_roll , Right , 10 , Signed 'I_set_roll = I_set_roll / 1024
D_set_roll = Gyrox * Hover_d
Shift D_set_roll , Right , 4 , Signed 'D_set_roll = D_set_roll / 16
Gyro_xangle = Gyro_xangle + Gyroy
Gyro_xangle = Gyro_xangle * Gyro_influence
Xangle = Accx * Acc_influence
Gyro_xangle = Gyro_xangle + Xangle
Nick_error = Gyro_xangle - Rc_set_nick
Nick_error_integral = Nick_error_integral + Nick_error
If Nick_error_integral >= 4000000 Then Nick_error_integral = 4000000
If Nick_error_integral < -4000000 Then Nick_error_integral = -4000000
P_set_nick = Nick_error * Hover_p
Shift P_set_nick , Right , 8 , Signed 'P_set_nick = P_set_nick / 256
I_set_nick = Nick_error_integral * Hover_i
Shift I_set_nick , Right , 10 , Signed 'I_set_nick = I_set_nick / 1024
D_set_nick = Gyroy * Hover_d
Shift D_set_nick , Right , 4 , Signed 'D_Set_nick = D_set_nick / 16
Elseif State = 0 Then
Roll_error_integral = 0
Nick_error_integral = 0
Roll_error_old = 0
Nick_error_old = 0
End If
If State >= 1 Then
Yaw_error = Gyroz
Yaw_error = Rc_set_yaw - Yaw_error
Yaw_error_integral = Yaw_error_integral + Yaw_error
If Yaw_error_integral >= 1024000 Then Yaw_error_integral = 1024000
If Yaw_error_integral < -1024000 Then Yaw_error_integral = -1024000
P_set_yaw = Yaw_error * Yaw_p
Shift P_set_yaw , Right , 9 , Signed 'P_set_yaw = P_set_yaw / 512
I_set_yaw = Yaw_error_integral * Yaw_i
Shift I_set_yaw , Right , 14 , Signed 'I_set_yaw = I_set_yaw / 16384
Elseif State = 0 Then
Yaw_error_integral = 0
End If
End Sub
Sub Drivemotors()
Vorwahl_pwm = Vorwahl + Pwm_mot_off
If State >= 1 Then
'--- Motor Rear Left ---
Motor_rear_left = Pwm_mot_off + Rc_set_throttle
Motor_rear_left = Motor_rear_left + Vorwahl
If Motor_rear_left > 55000 Then Motor_rear_left = 55000
Motor_rear_left = Motor_rear_left + P_set_roll
Motor_rear_left = Motor_rear_left + I_set_roll
Motor_rear_left = Motor_rear_left + D_set_roll
Motor_rear_left = Motor_rear_left - P_set_nick
Motor_rear_left = Motor_rear_left - I_set_nick
Motor_rear_left = Motor_rear_left - D_set_nick
Motor_rear_left = Motor_rear_left - P_set_yaw
Motor_rear_left = Motor_rear_left - I_set_yaw
If Motor_rear_left >= Pwm_mot_max Then Motor_rear_left = Pwm_mot_max
If Motor_rear_left < Vorwahl_pwm Then Motor_rear_left = Vorwahl_pwm
'--- Motor Rear Right ---
Motor_rear_right = Pwm_mot_off + Rc_set_throttle
Motor_rear_right = Motor_rear_right + Vorwahl
If Motor_rear_right >= 55000 Then Motor_rear_right = 55000
Motor_rear_right = Motor_rear_right - P_set_roll
Motor_rear_right = Motor_rear_right - I_set_roll
Motor_rear_right = Motor_rear_right - D_set_roll
Motor_rear_right = Motor_rear_right - P_set_nick
Motor_rear_right = Motor_rear_right - I_set_nick
Motor_rear_right = Motor_rear_right - D_set_nick
Motor_rear_right = Motor_rear_right + P_set_yaw
Motor_rear_right = Motor_rear_right + I_set_yaw
If Motor_rear_right >= Pwm_mot_max Then Motor_rear_right = Pwm_mot_max
If Motor_rear_right < Vorwahl_pwm Then Motor_rear_right = Vorwahl_pwm
'--- Motor Front Right ---
Motor_front_right = Pwm_mot_off + Rc_set_throttle
Motor_front_right = Motor_front_right + Vorwahl
If Motor_front_right >= 55000 Then Motor_front_right = 55000
Motor_front_right = Motor_front_right - P_set_roll
Motor_front_right = Motor_front_right - I_set_roll
Motor_front_right = Motor_front_right - D_set_roll
Motor_front_right = Motor_front_right + P_set_nick
Motor_front_right = Motor_front_right + I_set_nick
Motor_front_right = Motor_front_right + D_set_nick
Motor_front_right = Motor_front_right - P_set_yaw
Motor_front_right = Motor_front_right - I_set_yaw
If Motor_front_right >= Pwm_mot_max Then Motor_front_right = Pwm_mot_max
If Motor_front_right < Vorwahl_pwm Then Motor_front_right = Vorwahl_pwm
'--- Motor Front Left ---
Motor_front_left = Pwm_mot_off + Rc_set_throttle
Motor_front_left = Motor_front_left + Vorwahl
If Motor_front_left >= 55000 Then Motor_front_left = 550000
Motor_front_left = Motor_front_left + P_set_roll
Motor_front_left = Motor_front_left + I_set_roll
Motor_front_left = Motor_front_left + D_set_roll
Motor_front_left = Motor_front_left + P_set_nick
Motor_front_left = Motor_front_left + I_set_nick
Motor_front_left = Motor_front_left + D_set_nick
Motor_front_left = Motor_front_left + P_set_yaw
Motor_front_left = Motor_front_left + I_set_yaw
If Motor_front_left >= Pwm_mot_max Then Motor_front_left = Pwm_mot_max
If Motor_front_left < Vorwahl_pwm Then Motor_front_left = Vorwahl_pwm
Elseif State = 0 Then
Motor_rear_left = Pwm_mot_off
Motor_rear_right = Pwm_mot_off
Motor_front_right = Pwm_mot_off
Motor_front_left = Pwm_mot_off
End If
If Failsave < 15 Then
Tcd0_cca = Motor_front_left
Tcd0_ccb = Motor_front_right
Tcd0_ccc = Motor_rear_right
Tcd0_ccd = Motor_rear_left
Else
Tcd0_cca = Pwm_mot_off
Tcd0_ccb = Pwm_mot_off
Tcd0_ccc = Pwm_mot_off
Tcd0_ccd = Pwm_mot_off
End If
End Sub
Sub Init_mpu()
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 25 , #2 'Register 25 Sample Rate Divider (1..8 kHz)
I2cwbyte &B00000000 , #2 'Divider set to 1 (soll)
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 26 , #2 'Register 26 DLPF_CFG (digital lowpass filter) Configuration
I2cwbyte &B00000000 , #2 'Bits 0..2 = 000 - ACC:260Hz, 0.0ms; Gyro:256Hz, 0.98ms ( &B00000000 )
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 27 , #2 'Register 27 Gyro Configuration
I2cwbyte &B00001000 , #2 'Bits 3+4 = 00 - Full Scale Range: +/-250°/s
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 28 , #2 'Register 28 ACC Configuration
I2cwbyte &B00000000 , #2 'Bits 3+4 = 00 - Full Scale Range: +/-2g / No High Pass Filter
I2cstop #2 'stop condition
I2cstart #2 'start condition
I2cwbyte Mpuaddw , #2 'write adress of MPU-6050
I2cwbyte 107 , #2 'Register 107 Power Management 1
I2cwbyte &B00001011 , #2 'No Reset / No Sleep / No Cycle / Temp_Sens: Dis / Clock Source: Z-Gyro
I2cstop #2
End Sub
Sub Read_gyro()
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 67 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_gyrox(2) , Ack , #2
I2crbyte Tmp_gyrox(1) , Ack , #2
I2crbyte Tmp_gyroy(2) , Ack , #2
I2crbyte Tmp_gyroy(1) , Ack , #2
I2crbyte Tmp_gyroz(2) , Ack , #2
I2crbyte Tmp_gyroz(1) , Nack , #2
I2cstop #2
Gyrox = 0 - Gyrox
'(
Teile Gyro durch 32
Shift Gyrox , Right , 5 , Signed
Shift Gyroy , Right , 5 , Signed
Shift Gyroz , Right , 5 , Signed
')
Gyrox = Gyrox - Gyrox_offset
Gyroy = Gyroy - Gyroy_offset
Gyroz = Gyroz - Gyroz_offset
End Sub
Sub Read_acc()
I2cstart #2
I2cwbyte Mpuaddw , #2
I2cwbyte 59 , #2
I2crepstart #2
I2cwbyte Mpuaddr , #2
I2crbyte Tmp_accx(2) , Ack , #2
I2crbyte Tmp_accx(1) , Ack , #2
I2crbyte Tmp_accy(2) , Ack , #2
I2crbyte Tmp_accy(1) , Nack , #2
I2cstop #2
Accx = -accx
Accy = -accy
Shift Accx , Left , 3 , Signed
Shift Accy , Left , 3 , Signed
Accx = Accx - Accx_offset
Accy = Accy - Accy_offset
End Sub
Getreceiver:
If Channel >= 1 And Channel <= Maxrcchannel Then
Empftmp(channel) = Tcc0_cnt - 65092
Empf(channel) = Empftmp(channel)
End If
Tcc0_cnt = 65000
Incr Channel
Return
Detectrxpause:
Channel = 0
Return
Wenn nötig, könnte ich das Verhalten mal filmen, dass ihr euch ein besseres Bild davon machen könntet. 
Zitieren
Lesezeichen