|
|
现在正在搞平衡车，遇到了一些问题，请高手给讲解一下，谢谢
以下是程序，源自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 of angular 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 |
阿莫论坛20周年了！感谢大家的支持与爱护！！
一只鸟敢站在脆弱的枝条上歇脚，它依仗的不是枝条不会断，而是自己有翅膀，会飞。
|
发表于 2011-5-14 01:26:35
楼主
