平衡车高手高手请进
现在正在搞平衡车，遇到了一些问题，请高手给讲解一下，谢谢以下是程序，源自Elektor-Wheelie
但传感器我选用了ma7260 和 ewts84
5v参考电压
程序里有一些读不懂的地方如下面加**********段落。里面的35是如何得来的
还有Sub Algo函数也不理解其中17是表示什么？
应为在另一个开源项目中也看到相应的程序，但数值不同。
看了一些关于互不滤波的文章，Motor Output = Kp × (Angle) + Kd × (Angular Velocity)，但这里的Kp Kd 应该相加是1才对，为什么程序里不是。
修改相应参数后小车变化不大，依然抖动，不能平衡。
里面的竹市是我加上的，不知对否。
'---------------------------------------------------------------------------
'_____ _ _ _ _ _ _ _ _ _
' | __| |___| |_| |_ ___ ___ | | | | |_ ___ ___| |_|___
' | __| | -_| '_|_| . |_|| | | | | -_| -_| | | -_|
' |_____|_|___|_,_|_| |___|_| |_____|_|_|___|___|_|_|___|
'
' (c)2009 Elektor International Media B.V.
'---------------------------------------------------------------------------
'Version: EWOF100 - Elektor Wheelie Open Firmware 1.0.0
'---------------------------------------------------------------------------
'Purpose:
'Elektor-Wheelie motor control source code
'---------------------------------------------------------------------------
'Description:
'On power on the LEDs are flashing in the order green-yellow-red
'If initialization fails the red LED is blinking and the Wheelie is blocked.
'Blinking 2 times = gyro failure, 3 times = adxl failure.
'If initialization has been successful the 3 LEDs show the battery status:
'green = battery full, yellow = battery empty and red = battery dead.
'---------------------------------------------------------------------------
'Older sources:
'FW 2.6: Original Elekor firmware version 2.6 by Chris Krohne 2009
'Beta versions by G黱ter Gerold 2009
' beta.0:
' Parts with assembler code deleted;
' interrupts changed;
' overflows fixed;
' fixed bug at foot switch;
' code part MMode changed;
' code part PWM changed;
' unused variables deleted;
' unused constants deleted.
' beta.1:
' Code for telemetric purposes added (for LCD and PC-connection).
' beta.2:
' Faster control loop for better balance at inclination changes;
' new sensor calibration for every start (foot switch action);
' softer filter parameters.
' beta.3:
' Unused pins as inputs with pull-ups;
' no more speed measurement, because there is no real speed measure.
' beta.4:
' More comments;
' fixed bug with LEDs (red LED);
' changed order of code.
'---------------------------------------------------------------------------
'Changes Log:
'EWOF100: version 1.0.0
' Starting point for improvements by the community. Equivalent to beta.4.
'---------------------------------------------------------------------------
'To do:
'Improvement of steering control.
'Integration of real speed info.
'Implementation of a "security slow down mode".
'Etc.
'---------------------------------------------------------------------------
'Contact:
'Please post ideas, improvements and code into this thread in the Elektor forum:
'http://www.elektor.de/forum/foren-ubersicht/foren-zu-elektor-projekten/elektorwheelie/elektor-wheelie-open-firmware.1131470.lynkx
'G黱ter Gerold: tv@gerold-online.de
'---------------------------------------------------------------------------
$crystal = 16000000
$baud = 19200
$regfile = "m16def.dat"
$hwstack = 100 'Hardware stack
$swstack = 100 'Software stack
$framesize = 100 'Frame space
'设置输出端口
Pwm_al Alias Ocr1al
Pwm_bl Alias Ocr1bl
Led3 Alias Portc.3
Led2 Alias Portc.2
Led1 Alias Portc.1
Buzz Alias Portc.0
Cw_ccw_a Alias Portd.7
Cw_ccw_b Alias Portd.6
' 配置看门狗32ms
Config Watchdog = 32
Config Porta = Input
Config Portb = Input
Config Portc = Output
Config Portd = Output
'AD转换一次使用13个系统时钟，预分频128，则一次AD转换花费13*1/(1600000/128)=104us
'设置ADC参数，参考电压外部供给
Config Adc = Single , Prescaler = 128 , Reference = Off
Start Adc
'8bit T0ji计时器搐发中断服务程序，每16.32ms溢出 ，为了加速中断响应时间t0设置为100，计时器从100～255. 计算得155 x 1024 x 62,5 ns = 10 ms
'Timer0 as a 8 bit timer triggers interrupt service routine On timer0 every 16,32ms (on overflow).
'To speed up the interrupt intervall Timer0 is preset to 100. Now counting from 100 to 255.
'=> Calcutulation: 155 x 1024 x 62,5 ns = 10 ms
Config Timer0 = Timer , Prescale = 1024
On Timer0 Ontimer0
'T1用于产生前进和后退的PWM脉冲，其中一个是反转
'Timer1 is used to generate both PWM signals (backward and forward).
'One of them is inverted.
Tccr1a = &B10110001 '8位相位修正PWM
Tccr1b = 2
Ocr1al = 255
Dim Ad_adxl As Long
Dim Ad_gyro As Long
Dim Ad_batt As Integer
Dim Ad_swi As Integer
Dim Total_adxl_gyro As Long
Dim Average_gyro As Long
Dim Average_batt As Long
Dim Drivea As Integer
Dim Driveb As Integer
Dim Buf1 As Long
Dim Buf As Long
Dim Buf2 As Long
Dim Tilt_angle As Long
Dim Drive_a As Integer
Dim Drive_b As Integer
Dim Drivespeed As Long
Dim Steeringsignal As Long
Dim Anglecorrection As Long
Dim Angle_rate As Long
Dim Balance_moment As Long
Dim Mmode As Byte
Dim Drive_sum As Long
Dim Ad_rocker As Integer
Dim Timeout As Long
Dim Rockersq As Long
Dim Rocker_zero As Integer
Dim Adxl_zero As Word
Dim Gyro_zero As Word
Dim Errno As Byte
Dim Adxl_offset As Integer
Const Mdrivesumlimit = 20000
Const Total_looptime = 300 ' Looptime for filters
Const Total_looptime10 = 30 ' Looptime for filters
Const _run = 1
Const _standby = 0
Const _down = 3
Const Sw_down = 50 'Thershold for foot switch
Const Critical = 80 'Time from releasing the foot switch until begin to stop the Wheelie 从释放脚踏开关到停车的时间
Const Max_pwm = 180 'Maximum motor power
Const Mmax_pwm = -180 'Maximum motor power
Const Battdead = 960 * Total_looptime
Const Battok = 1020 * Total_looptime
Declare Sub Get_tilt_angle
Declare Sub Set_pwm
Declare Sub Algo
Declare Sub Process
Declare Sub Checkbatt
Declare Sub Err_value
Declare Sub Init
Declare Sub Steering
Pwm_al = 255
Pwm_bl = 0
Average_batt = Battok
Mmode = _standby
'LEDshow on start
Set Led1
Waitms 150
Set Led2
Waitms 150
Set Led3
Set Buzz
Waitms 250
Reset Led1
Reset Led2
Reset Led3
Reset Buzz
'---------------------------------------------------------------------------
'Checking foot switch:
'When the Wheelie is switched on the foot switch should be closed (not pressed down).
'Otherwise the Wheelie would start. Therefore the foot switch is read 10 times
'and the mean of this values is compared with a threshold of 100.
'If this mean is smaller than all is okay. Otherwise an error occurs..
'---------------------------------------------------------------------------
Ad_swi = 0
For Buf = 1 To 10 'Calculate mean out of 10 measurements
Ad_swi = Ad_swi + Getadc(4)
Waitms 1
Next Buf
Ad_swi = Ad_swi / 10
Waitms 2
If Ad_swi < 100 Then 'If foot switch is pressed: error
Errno = 4
Goto Err_value
End If
'Print Ad_swi
Gosub Init
Enable Interrupts
Enable Timer0
'---------------------------------------------------------------------------
'Main Loop:
'The right space for extending with code.
'---------------------------------------------------------------------------
Do
'Do nothing (until now)s
Loop
'---------------------------------------------------------------------------
'Ontimer0:
'This service routine contains control logic and is started every 10ms.
'Exact time: 1/16 MHz * 1024 * (255-100) = 9,92 ms
'---------------------------------------------------------------------------
Ontimer0:
Reset Watchdog
Timer0 = 100
Gosub Get_tilt_angle
Gosub Algo
Gosub Steering
Gosub Set_pwm
Gosub Checkbatt
Gosub Process
Return
'---------------------------------------------------------------------------
'Get_tilt_angle:
'Measurement of all analog values (5 values).
'Using 104 祍 per measurement total time is 520 祍.
'The values are used to build moving averages.
'---------------------------------------------------------------------------
Sub Get_tilt_angle
Ad_gyro = Getadc(2)
'Ad_gyro = 1024 - Ad_gyro 'Inverted value for Ad_gyro
' Ad_gyro = Ad_gyro
Ad_adxl = Getadc(1)
'Ad_adxl = 1024 - Ad_adxl
Ad_batt = Getadc(6)
Ad_rocker = Getadc(5)
Ad_swi = 1024 - Getadc(4) 'Inverted value for Ad_swi
'Subtract 1/300th of the value
Buf = Total_adxl_gyro / Total_looptime
Total_adxl_gyro = Total_adxl_gyro - Buf
'Filter for ADXL
Buf = Ad_adxl - Adxl_zero 'Deviation from horizontal position
Buf = Buf + Adxl_offset 'Ideal value for Adxl_offset would be zero
Total_adxl_gyro = Total_adxl_gyro + Buf 'A new 1/300th value is added
'Filter for gyro
Buf1 = Average_gyro / Total_looptime
Average_gyro = Average_gyro - Buf1 'Subtract 1/300th of the value
Average_gyro = Average_gyro + Ad_gyro 'A new 1/300th value is added
Buf1 = Average_gyro / Total_looptime10 'Calculation ofangular rate
****************************************************************************************************************
Buf = Ad_gyro * 10
Buf1 = Buf1 - Buf 'Deviation from zero
Buf1 = Buf1 * 35
Buf1 = Buf1 / 100 '3.5 * deviation rate
Angle_rate = Buf1 'with moving average over 300 measurements
'Calculation of Tilt_angle
'Subtract the contained part of Gyro?
'A bit unclear...
Buf1 = Angle_rate * 1
Total_adxl_gyro = Total_adxl_gyro + Buf1
Tilt_angle = Total_adxl_gyro / Total_looptime10
End Sub
******************************************************************************************************************
'---------------------------------------------------------------------------
'Algo:
'Calculation of motor current out of sensor values.
'---------------------------------------------------------------------------
Sub Algo '算法
Buf = Tilt_angle
Buf = Buf * 17 '倾斜角度
Buf1 = Angle_rate * 17 '加速率
Balance_moment = Buf1 + Buf '平衡时刻
Drive_sum = Drive_sum + Balance_moment '驱动量=先前值+平衡时刻
If Drive_sum > 55000 Then 'Limiting Drive_sum
Drive_sum = 55000
End If
If Drive_sum < -55000 Then 'Limiting Drive_sum
Drive_sum = -55000
End If
Buf = Drive_sum / 155
Buf1 = Balance_moment / 165
Drivespeed = Buf + Buf1 '驱动速度
End Sub
'---------------------------------------------------------------------------
'Steering:
'Calculation of steering values.
'---------------------------------------------------------------------------
Sub Steering
Rockersq = Rocker_zero - Ad_rocker
Buf1 = Drive_sum / 20000
'Print "rocker" ; Rocker_zero ; Ad_rocker 'Steering depends on speed
Buf1 = Abs(buf1)
If Buf1 < 1 Then Buf1 = 1
Rockersq = Rockersq / Buf1
'Some safety lines, limits steering: Drive_sum 55000 max = +-5 Buf1 = Drive_sum / 2000
Buf1 = Abs(buf1)
If Buf1 > 22 Then Buf1 = 22
Buf1 = 27 - Buf1
Buf2 = Buf1 * -1
If Rockersq > Buf1 Then Rockersq = Buf1
If Rockersq < Buf2 Then Rockersq = Buf2
Steeringsignal = Rockersq
Drive_a = Drivespeed - Steeringsignal
Drive_b = Drivespeed + Steeringsignal
End Sub
'---------------------------------------------------------------------------
'Set_pwm:
'Limiting PWM and set direction flags.
'---------------------------------------------------------------------------
Sub Set_pwm
'Limiting PWM '限制速度
If Drive_a > Max_pwm Then
Drive_a = Max_pwm
Elseif Drive_a < Mmax_pwm Then
Drive_a = Mmax_pwm
End If
If Drive_b > Max_pwm Then
Drive_b = Max_pwm
Elseif Drive_b < Mmax_pwm Then
Drive_b = Mmax_pwm
End If
'Set direction flag '设置 方向标志
If Drive_a < 0 Then
Cw_ccw_a = 1
Else
Cw_ccw_a = 0
End If
If Drive_b < 0 Then
Cw_ccw_b = 1
Else
Cw_ccw_b = 0
End If
Drivea = Abs(drive_a) '返回绝对值速度
Driveb = Abs(drive_b)
'Print "dribca=" ; Drivea ; "dribcb=" ; Driveb ;
'Inverse signal with 180?phaseshift to PWMb
Pwm_al = 255 - Drivea '180度换向
Pwm_bl = Driveb
End Sub
'---------------------------------------------------------------------------
'CheckBatt: 电池检测
'Battery state (voltage) is signaled with the 3 LEDs.
'---------------------------------------------------------------------------
Sub Checkbatt
'Moving average of battery voltage
Buf1 = Average_batt / Total_looptime
Average_batt = Average_batt - Buf1
Average_batt = Average_batt + Ad_batt
'Set the right LED
If Average_batt > Battok Then
Set Led1
Reset Led2
Reset Led3
End If
If Average_batt < Battok Then
Set Led2
Reset Led3
Reset Led1
End If
If Average_batt < Battdead Then
Reset Led2
Reset Led1
Set Led3
End If
End Sub
'---------------------------------------------------------------------------
'Process:
'Processing Wheelie states.
'---------------------------------------------------------------------------
Sub Process
Select Case Mmode
Case _standby 'Foot switch is released:wating...脚踏开关释放则等待
Drive_a = 0
Drive_b = 0
Drivespeed = 0
Anglecorrection = 0
Drive_sum = 0
Total_adxl_gyro = 0
Tilt_angle = 0
Timeout = 0
If Ad_swi > Sw_down Then 'If foot switch is pressed: change Mmode to run
Gosub Init 'Initialize sensors脚踏开关压下则进入运行模式初始化传感器
Mmode = _run
Timeout = 0
End If
Case _run 'Foot switch is pressed: action... 脚踏开关压下：运行
If Ad_swi < Sw_down Then 'If foot switch is released long enough: change Mmode to down 如果被释放时间过长则进入减速模式
Incr Timeout
If Timeout > Critical Then
Mmode = _down
Timeout = 0
End If
Else
Timeout = 0
End If
Case _down 'Motors slowdown slowly 减速
Stop Watchdog
For Buf1 = 1 To 255
If Drive_a = 0 And Drive_b = 0 Then
Exit For
End If
If Drive_a > 0 Then
Decr Drive_a
End If
If Drive_a < 0 Then
Incr Drive_a
End If
If Drive_b > 0 Then
Decr Drive_b
End If
If Drive_b < 0 Then
Incr Drive_b
End If
Waitms 25 'Loop speed
Gosub Set_pwm
Next Buf1
Mmode = _standby
Start Watchdog
Case Else 'If an unknown state occurs then slow down如果一个未知状态发生则减速
Mmode = _down
End Select
End Sub
'---------------------------------------------------------------------------
'Init:
'Calibration: Calculation of steering mid and platform horizontal position.
'---------------------------------------------------------------------------
Sub Init
'Calculate mid position of steering (Steering_zero) 计算方向中值
Stop Watchdog
Ad_rocker = 0
For Buf = 1 To 10 'Mean out of 10 values
Ad_rocker = Ad_rocker + Getadc(5)
Waitms 1
Next Buf
Rocker_zero = Ad_rocker / 10
'Print Rocker_zero ; "";
'Calculate gyro horizontal position of platform计算陀螺仪平台水平位置
Ad_gyro = 0
For Buf = 1 To 100 'Mean out of 100 values
Buf1 = Getadc(2)
' Buf1 =Buf1
Ad_gyro = Ad_gyro + Buf1
Waitms 2
Next Buf
Gyro_zero = Ad_gyro / 100
Average_gyro = Total_looptime * Gyro_zero 'Preset Average_gyro
'Print Gyro_zero ; "";
'Calculate ADXL horizontal position of platform计算加速计平台水平位置
Ad_adxl = 0
For Buf = 1 To 100 'Mean out of 100 values
Buf1 = Getadc(1)
Ad_adxl = Ad_adxl + Buf1
Waitms 2
Next Buf
Adxl_zero = Ad_adxl / 100
'Print Adxl_zero ; "";
'Check if values are in tolerable range 检查陀螺仪/加速计值是否在允许范围内
Start Watchdog
If Adxl_zero < 300 Then
Errno = 3
Goto Err_value
End If
If Adxl_zero > 600 Then
Errno = 3
Goto Err_value
End If
If Gyro_zero < 350 Then
Errno = 2
Goto Err_value
End If
If Gyro_zero > 750 Then
Errno = 2
Goto Err_value
End If
End Sub
'---------------------------------------------------------------------------
'Err_value:
'On error the program processes this routine endlessly.
'Switch the Wheelie off to reset.
'---------------------------------------------------------------------------
Sub Err_value
Stop Watchdog
Do
'Print "eror=" ; Errno
For Buf = 1 To Errno
Set Led3
Waitms 350
Reset Led3
Waitms 350
Next Buf
Wait 2
Loop
End Sub 自己顶一下 用VB编的？ 是的，德国人的，感觉简单实用就行。
有专门的论坛，但都是鸟语看不懂。
高手能帮看看吗？ 没人能回答吗？ 定 定，木有人回答吗？ 高手没在，我也想做，正在学习单片机，希望看到高手支持。 这个是VB?不应该是单片机控制的吗？ 17可能是系数。有的版本是19.
零点不固定。另外可能 Sub Get_tilt_angle 有错。用示波器发现Ad_adxl 变化时。pwm没变化。 楼上也在研究，是不是老外的程序没有完全提供正确的版本，
不妨将你的研究心得与大家分享一下。
感觉老外的程序比较简单。
页:
[1]