|
|
发表于 2013-6-20 10:34:27
|
显示全部楼层
'zzaag Firmware for fun-components Controller UC01'
' Release A2
$crystal = 16000000
$baud = 19200
Pwm_al Alias Ocr1al
Pwm_bl Alias Ocr1bl
Vreg Alias Portd.2
Buzz Alias Portc.0
Led1 Alias Portc.1
Led2 Alias Portc.2
Led3 Alias Portc.3
Led4 Alias Portc.4
Led5 Alias Portc.6
Cw_ccw_a Alias Portd.7
Cw_ccw_b Alias Portd.6
Config Porta = Input
Config Portb = Input
Config Portc = Output
Config Portd = Output
Config Adc = Single , Prescaler = 128 , Reference = Off
Start Adc
Config Timer0 = Timer , Prescale = 1024
On Timer0 Tinter
Portb = 3
Dim Buzzer As Byte
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 Buf5 As Integer
Dim Tilt_angle As Long
Dim Drive_a As Integer
Dim Drive_b As Integer
Dim Drive_as As Integer
Dim Drive_bs As Integer
Dim Tcount As Integer
Dim Drivespeed As Integer
Dim Steeringsignal As Long
Dim Anglecorrection As Long
Dim Angle_rate As Long
'Dim Voltage As Long
Dim Balance_moment As Long
Dim Overspeed As Long
Dim Mmode As Byte
Dim Overspeed_flag As Byte
Dim Drive_sum As Long
Dim Ad_rocker As Integer
Dim Blink_flag As Byte
Dim Timeout As Long
Dim Delta As Byte
Dim Buf3 As Long
Dim Rockersq As Long
Dim Rocker_zero As Integer
Dim Adxl_zero As Word
Dim Gyro_zero As Word
Dim Loopct As Integer
Const First_adc_input = 0
Const Last_adc_input = 6
Const Adc_vref_type = $0x40
Const Mdrivesumlimit = -43000
Const Total_looptime = 500 ' / / Looptime For Filters
Const Total_looptime10 = 50 ' / / Looptime For Filters
Const Adxl_offset = -13 ' / / Stick Angle Offset
Const _run = 1
Const _standby = 0
Const _warmup = 2
Const Safespeed = 5000
Const Msafespeed = -5000
Const Sw_down = 50
Const Critical = 80
Const Battok = 1023
Const Batt_low = 990
Const Max_pwm = 210
Const Mmax_pwm = -210
Declare Sub Get_bat_volt
Declare Sub Get_tilt_angle
Declare Sub Set_pwm
Declare Sub Algo
Declare Sub Process
Declare Sub Ini
Declare Sub Getspeedlimit
Declare Sub Err_eeprom
' motorcontroller disabled / PWM-Signals to zero'
Set Vreg
Pwm_al = 255
Pwm_bl = 0
'Init Variables'
Gosub Ini
'--------------------------------------------------------------'
'check calibrate Jumper'
If Pinb.0 = 0 Then
'calibrate'
Print "CALIB-MODE"
'give some signs
For Buf = 1 To 4
Set Buzz
Waitms 75
Reset Buzz
Waitms 75
Next Buf
Waitms 500
Buf1 = 0
For Buf = 1 To 10
'read AD-Channel ad_gyro'
Buf3 = Getadc(3)
Buf1 = Buf1 + Buf3
Waitms 100
Next Buf
'calculate middle'
Buf5 = Buf1 / 10
Delta = High(buf5)
'store data in eeprom'
Writeeeprom Delta , 2
Writeeeprom Delta , 4
Delta = Low(buf5)
Writeeeprom Delta , 3
Writeeeprom Delta , 5
Buf1 = 0
For Buf = 1 To 10
'read AD-channel ad_adxl'
Buf3 = Getadc(1)
Buf1 = Buf1 + Buf3
Waitms 100
Next Buf
Buf5 = Buf1 / 10
Delta = High(buf5)
'store data in eeprom'
Writeeeprom Delta , 6
Writeeeprom Delta , 8
Delta = Low(buf5)
Writeeeprom Delta , 7
Writeeeprom Delta , 9
End If
'-----------------------------------------------------------------'
'-----------------------------------------------------------------'
'check Bootloader Jumper'
If Pinb.1 = 0 Then
'Bootloader'
Print "Entering bootloader"
'give some signs
For Buf = 1 To 5
Set Buzz
Waitms 45
Reset Buzz
Waitms 75
Next Buf
'jump to bootload code'
jmp $3c00
End If
'-----------------------------------------------------------------'
'-----------------------------------------------------------------'
'read Calib.Data (pair 1)
Readeeprom Delta , 2
Buf = Delta * 256
Readeeprom Delta , 3
Buf = Buf + Delta
'Buf = 527
'read Calib.Data (pair 2)
Readeeprom Delta , 4
Buf3 = Delta * 256
Readeeprom Delta , 5
Buf3 = Buf3 + Delta
'Buf3 = 527
'set gyro_zero value'
Gyro_zero = Buf
'Datapairs not equal-> Epprom-err'
If Buf3 <> Buf Then
Goto Err_eeprom
End If
'or empty eeporm (default value) ?
If Buf3 = 65535 Then
Goto Err_eeprom
End If
'read Calib.Data (pair 1)
Readeeprom Delta , 6
Buf = Delta * 256
Readeeprom Delta , 7
Buf = Buf + Delta
'read Calib.Data (pair 2)
Readeeprom Delta , 8
Buf3 = Delta * 256
Readeeprom Delta , 9
Buf3 = Buf3 + Delta
'Buf = 515
'Buf3 = 515
'set adxl_zero value'
Adxl_zero = Buf
'Datapairs not equal-> Epprom-err'
If Buf3 <> Buf Then
Goto Err_eeprom
End If
'or empty eeporm (default value) ?
If Buf3 = 65535 Then
Goto Err_eeprom
End If
'set average_gyro
Average_gyro = Total_looptime * Gyro_zero
'---------------------------------------------------------'
'---------------------------------------------------------'
'Get Steering_zero Position'
Ad_rocker = 0
For Buf = 1 To 10
Ad_rocker = Ad_rocker + Getadc(5)
Waitms 5
Next Buf
'calculate middel'
Rocker_zero = Ad_rocker / 10
'----------------------------------------------------------'
'----------------------------------------------------------'
' Give some beeps
Set Buzz
Waitms 20
Reset Buzz
Waitms 50
Set Buzz
Waitms 20
Reset Buzz
'enable Motorcontroller'
Reset Vreg
Enable Interrupts
Enable Timer0
'main loop
S1:
' nothing to do, all work is done in the interrupt ;)
Goto S1
'----------------------------------------------------------'
'----------------------------------------------------------'
Sub Get_bat_volt
Buf = Average_batt / Total_looptime
Average_batt = Average_batt - Buf
Average_batt = Average_batt + Ad_batt
'check Batt Voltage
Buf = Batt_low * Total_looptime
If Average_batt < Buf Then
Set Buzz
End If
End Sub
'----------------------------------------------------------'
'----------------------------------------------------------'
Sub Set_pwm
'Limiting PWM'
If Drive_a > Max_pwm Then
Drive_a = Max_pwm
End If
If Drive_a < Mmax_pwm Then
Drive_a = Mmax_pwm
End If
'set direction bit'
If Drive_a < 0 Then
Drivea = Drive_a * -1
Cw_ccw_a = 1
End If
If Drive_a >= 0 Then
Drivea = Drive_a
Cw_ccw_a = 0
End If
'Inverse signal to have 180?phaseshift to PWMB
Pwm_al = 255 - Drivea
'Limiting PWM'
If Drive_b > Max_pwm Then
Drive_b = Max_pwm
End If
If Drive_b < Mmax_pwm Then
Drive_b = Mmax_pwm
End If
'set direction bit'
If Drive_b < 0 Then
Driveb = Drive_b * -1
Cw_ccw_b = 1
End If
If Drive_b >= 0 Then
Driveb = Drive_b
Cw_ccw_b = 0
End If
Pwm_bl = Driveb
End Sub
'----------------------------------------------------------'
'----------------------------------------------------------'
Sub Algo
Buf = Tilt_angle + Anglecorrection
Buf = Buf * 17
Buf1 = Angle_rate * 20
'calculate balance moment
Balance_moment = Buf1 + Buf
'check speedlimit
Gosub Getspeedlimit
'calculate drive_sum
Drive_sum = Drive_sum + Balance_moment
' limitting
If Drive_sum > 55000 Then
Drive_sum = 55000
End If
If Drive_sum < -55000 Then
Drive_sum = -55000
End If
'calculate drive speed'
Buf = Drive_sum / 125
Buf1 = Balance_moment / 125
Drivespeed = Buf + Buf1
End Sub
'----------------------------------------------------------'
'----------------------------------------------------------'
Sub Getspeedlimit
If Drive_sum < 0 Then
If Drive_sum < Mdrivesumlimit Then
Anglecorrection = -13
Buzzer = 1
Set Buzz
Else
Anglecorrection = 0
End If
End If
End Sub
'----------------------------------------------------------'
'----------------------------------------------------------'
Sub Process
Tcount = Tcount + 1
'do some logs or something else
If Tcount > 30 Then
Tcount = 1
'Print Tilt_angle ; " ";
'Print Ad_adxl ; " ";
'Print Ad_gyro ; " ";
Print Average_batt ; " ";
Print Ad_batt;
'Print Average_gyro ; " ";
'Print Mmode;
Print
End If
'calculate steering'
Rockersq = Rocker_zero - Ad_rocker
If Rockersq > 0 Then
'make it progressiv
Rockersq = Rockersq * Rockersq
End If
If Rockersq < 0 Then
Rockersq = Rockersq * Rockersq
Rockersq = Rockersq * -1
End If
'Steering depends even on speed'
Buf1 = Drive_sum / 600
If Buf1 < 0 Then
Buf1 = Buf1 * -1
End If
Buf1 = Buf1 + 77
Rockersq = Rockersq / Buf1
'some safety lines, limits the max. steering
If Rockersq > 25 Then Rockersq = 25
If Rockersq < -25 Then Rockersq = -25
'make the steering smooth
If Steeringsignal < Rockersq Then
Steeringsignal = Steeringsignal + 4
End If
If Steeringsignal > Rockersq Then
Steeringsignal = Steeringsignal - 4
End If
'set speed and steering
Drive_a = Drivespeed - Steeringsignal
Drive_b = Drivespeed + Steeringsignal
'need some seconds to compensate the gyro temp-drift
If Mmode = _warmup Then
Drive_a = 0
Drive_b = 0
Drive_as = 0
Drive_bs = 0
Drivespeed = 0
Anglecorrection = 0
Overspeed = 0
Drive_sum = 0
Total_adxl_gyro = 0
Tilt_angle = 0
If Loopct > 1100 Then
Mmode = _standby
' Give some beeps
Set Buzz
Waitms 20
Reset Buzz
Waitms 50
Set Buzz
Waitms 20
Reset Buzz
End If
End If
If Mmode = _standby Then
Drive_a = 0
Drive_b = 0
Drive_as = 0
Drive_bs = 0
Drivespeed = 0
Anglecorrection = 0
Overspeed = 0
Drive_sum = 0
Total_adxl_gyro = 0
Tilt_angle = 0
Buf2 = Ad_adxl - Adxl_zero
Buf2 = Buf2 + Adxl_offset
Buzzer = 0
Timeout = 0
'stand on platform
If Ad_swi > Sw_down Then
Mmode = _run
End If
End If
If Mmode = _run Then
'check platform buttons
If Ad_swi > Sw_down Then
'Person on board,
'buzzer still on?
If Buzzer = 1 Then
Buzzer = 0
End If
End If
If Ad_swi < Sw_down Then
'no Person on baord!, check savespeed
If Drive_sum < 0 Then
If Drive_sum < Msafespeed Then
Buzzer = 1
End If
If Drive_sum > Msafespeed Then
Buzzer = 0
End If
End If
If Drive_sum > 0 Then
If Drive_sum > Safespeed Then
Buzzer = 1
End If
If Drive_sum < Safespeed Then
Buzzer = 0
End If
End If
If Buzzer = 1 Then
If Timeout > Critical Then
Mmode = _standby
End If
End If
End If
If Buzzer = 0 Then
Timeout = 0
End If
Timeout = Timeout + Buzzer
End If
End Sub
'-----------------------------------------------------------------------'
Sub Ini
$asm
ldi r16,0
Out Porta , R16
Out Ddra , R16
'ldi r16,$e2
'Out Portb , R16
'ldi r16,$e3
'Out Ddrb , R16
ldi r16,0
Out Portc , R16
ldi r16,255
Out Ddrc , R16
ldi r16,$34
Out Portd , R16
ldi r16,$ff
Out Ddrd , R16
ldi r16,$b1 'b1=8bit ,b2=9bit,b3=10bit
Out Tccr1a , R16
ldi r16,$01 ' 01 no prescaler!
Out Tccr1b , R16
ldi r16,0
Out Tcnt1h , R16
Out Tcnt1l , R16
Out Icr1h , R16
Out Icr1l , R16
Out Ocr1ah , R16
ldi r16,$ff
'Out Ocr1ah , R16
Out Ocr1al , R16
ldi r16,0
'Out Ocr1bh , R16
ldi r16,0
Out Ocr1bl , R16
Out Assr , R16
Out Tccr2 , R16
Out Tcnt2 , R16
Out Ocr2 , R16
$end Asm
Average_batt = Total_looptime * Battok
Total_adxl_gyro = 0
Average_gyro = 0
'Average_batt = 0
Drivea = 0
Driveb = 0
Tilt_angle = 0
Drive_a = 0
Drive_b = 0
Drivespeed = 0
Steeringsignal = 0
Anglecorrection = 0
Angle_rate = 0
'Voltage = 0
Balance_moment = 0
Overspeed = 0
Mmode = _warmup
Overspeed_flag = 0
Drive_sum = 0
Ad_rocker = 0
Tcount = 0
Steeringsignal = 0
Drive_as = 0
Drive_bs = 0
Timeout = 0
Delta = 0
Loopct = 0
End Sub
'----------------------------------------------------'
Tinter:
Disable Interrupts
Gosub Get_bat_volt
Gosub Get_tilt_angle
Gosub Algo
Gosub Process
Gosub Set_pwm
Buzz = Buzzer
Timer0 = 100
Enable Interrupts
Return
'----------------------------------------------------'
Sub Err_eeprom
S3:
For Buf = 1 To 6
Set Buzz
Waitms 75
Reset Buzz
Waitms 75
Next Buf
Wait 2
Goto S3
End Sub
'----------------------------------------------------'
Sub Get_tilt_angle
Ad_gyro = Getadc(3)
Ad_adxl = Getadc(1)
Ad_batt = Getadc(6)
Ad_rocker = Getadc(5)
Ad_swi = 1024 - Getadc(4)
Buf = Total_adxl_gyro / Total_looptime
Total_adxl_gyro = Total_adxl_gyro - Buf
' ADXL part
Buf = Ad_adxl - Adxl_zero
Buf = Buf + Adxl_offset
Total_adxl_gyro = Total_adxl_gyro + Buf
' Gyro part
Buf1 = Average_gyro / Total_looptime
Average_gyro = Average_gyro - Buf1
Average_gyro = Average_gyro + Ad_gyro
Buf1 = Average_gyro / Total_looptime10
' calculate the Angle Rate
Buf = Ad_gyro * 10
Buf1 = Buf1 - Buf
Buf1 = Buf1 * 35
Buf1 = Buf1 / 100
Angle_rate = Buf1
' calculate the Tilt Angle
Total_adxl_gyro = Total_adxl_gyro + Angle_rate
Tilt_angle = Total_adxl_gyro / Total_looptime10
Loopct = Loopct + 1
Return
End Sub
End |
|
