logo elektroda
logo elektroda
X
logo elektroda
REKLAMA
REKLAMA
Adblock/uBlockOrigin/AdGuard mogą powodować znikanie niektórych postów z powodu nowej reguły.

[BASCOM]Autokalibracja dla ADC

DarcoDarini 20 Sty 2010 17:59 1449 0
REKLAMA
  • #1 7564734
    DarcoDarini
    Poziom 11  
    Oto kod
    Kod:
    
    '4x4 Line Follower
    'Chibots line following Contest 2003
    'Wright Hobbies, LLC Copyright 2003
    'eddy@wrighthobbies.com
    
    'Variables
    'Line Detectors
    Dim Lineflag As Byte
    Dim I As Byte , X As Byte
    Dim Adcvalue As Word , Channel As Byte , Progspeed As Byte
    Dim Progsteer As Byte
    Dim Lastadc(5) As Word
    Dim Calhi(5) As Word , Callow(5) As Word , Threshold(5) As Word
    Dim Temp As Word
    Dim Lastlineflag As Byte
    Dim Overshoot As Boolean
    Dim Lostline As Word
    Dim Flagleft As Boolean
    Dim Flagright As Boolean
    
    
    
    'Constants
    Const Center = 95
    Const Smallleft = 94
    Const Mediumleft = 85
    Const Largeleft = 75
    Const Hardleft = 70
    Const Smallright = 96
    Const Mediumright = 107
    Const Largeright = 117
    Const Hardright = 122
    
    'For Calibration
    Const Fullright = 135
    Const Fullleft = 60
    
    
    Const Neutral = 100
    Const Veryslow = 95
    Const Slow = 92
    Const Mediumslow = 91
    Const Medium = 90
    Const Mediumfast = 89
    Const Fast = 88
    Const Superfast = 80
    Const Slowreverse = 110
    Const Fastreverse = 150
    
    'Declarations
    Declare Sub Doadc()
    Declare Sub Waitforbutton()
    
    'Aliases
    Redled Alias Portd.6
    Greenled Alias Portd.5
    Buttonleft Alias Pinb.1
    Buttonright Alias Pinb.2
    
    Config Adc = Single , Prescaler = Auto , Reference = Avcc
    
    Config Servos = 2 , Servo1 = Portb.0 , Servo2 = Portd.7 , Reload = 10
    
    Config Portb = Output
    Config Portd = Output
    
    Config Pinb.1 = Input
    Config Pinb.2 = Input
    
    Portb.1 = 1
    Portb.2 = 1
    
    Servo(1) = Center
    
    Enable Interrupts
    
    Start Timer1
    
    Reset Greenled
    Reset Redled
    
    Progspeed = 0
    X = 0
    
    Do 'Wait for program the speed setting
    
    Debounce Buttonleft , 0 , Program , Sub
    Debounce Buttonright , 0 , Complete , Sub
    
    Loop Until X = 1
    
    Set Redled
    Print " " ; "Speed selection: " ; Progspeed
    
    Reset Redled
    Reset Greenled
    Waitms 100
    Set Redled
    Set Greenled
    Waitms 100
    Reset Redled
    Reset Greenled
    Waitms 100
    Set Redled
    Set Greenled
    
    Waitforbutton
    Progsteer = Progspeed
    
    Progspeed = Progspeed * 2
    
    
    Servo(1) = Center
    
    
    Channel = 0
    
    Start Adc
    'Initial Read ignored
    Adcvalue = Getadc(0)
    
    'Calibrate sensors
    Channel = 0
    'Print "Starting Calibration"
    
    For I = Fullleft To Fullright Step 2 'Begin scan for auto-calibration of sensors
    
    Servo(1) = I
    Waitms 50
    For Channel = 0 To 4
    Lastadc(channel + 1) = Getadc(channel)
    Next Channel
    
    For X = 1 To 5 'Scan through adc values and capture the highs and lows
    If Calhi(x) = 0 Then
    Calhi(x) = Lastadc(x)
    Callow(x) = Lastadc(x)
    End If
    If Lastadc(x) > Calhi(x) Then Calhi(x) = Lastadc(x)
    If Lastadc(x) < Callow(x) Then Callow(x) = Lastadc(x)
    Next X
    
    Next I
    
    Servo(1) = Center
    
    Channel = 0
    
    
    
    For I = 1 To 5 'Determine the threshold for line/no lin
    Threshold(i) = Calhi(i) - Callow(i)
    Threshold(i) = Threshold(i) / 2
    Threshold(i) = Calhi(i) - Threshold(i)
    Threshold(i) = 700
    Next I
    
    Servo(1) = Center
    
    Waitforbutton
    
    '******************************
    'Main loop
    '******************************
    
    Servo(2) = Neutral
    'Shake for 2 seconds before going
    For I = 1 To 200
    Servo(1) = Center - 1
    Waitms 5
    Servo(1) = Center + 1
    Waitms 5
    Next I
    
    Servo(2) = Fast
    
    Do 'Main Program Loop
    
    Doadc 'Check the sensors
    If Lastadc(1) < Threshold(1) Then Set Lineflag.0 Else Reset Lineflag.0
    If Lastadc(2) < Threshold(2) Then Set Lineflag.1 Else Reset Lineflag.1
    If Lastadc(3) < Threshold(3) Then Set Lineflag.2 Else Reset Lineflag.2
    If Lastadc(4) < Threshold(4) Then Set Lineflag.3 Else Reset Lineflag.3
    If Lastadc(5) < Threshold(5) Then Set Lineflag.4 Else Reset Lineflag.4
    
    'Print Bin(lineflag)
    
    If Overshoot = 0 Then Lastlineflag = Lineflag 'Save results if we're not overshooting the corner
    
    Select Case Lineflag
    
    Case &B00000 'No line
    'If the bot loses the line, make steering no changes
    Servo(2) = Slow - Progspeed
    If Lastlineflag < 4 Then
    Overshoot = 1 'Overshot a righthand corner
    Elseif Lastlineflag > 4 Then
    Overshoot = 1 'Overshot a left hand corner
    End If
    
    Case &B00100 'Line under center sensor
    Lostline = 0
    Overshoot = 0
    Servo(1) = Center
    Servo(2) = Mediumfast - Progspeed
    
    'Right Sensors
    
    Case &B00110
    
    Overshoot = 0
    Servo(1) = Smallright
    Servo(2) = Mediumfast - Progspeed
    
    Case &B00010
    Overshoot = 0 'Line under midright sensor
    Servo(1) = Mediumright
    Servo(2) = Mediumfast - Progspeed
    
    Case &B00011 'Line between midright and right sensor
    If Overshoot = 0 Then
    Servo(1) = Largeright
    Servo(2) = Medium - Progspeed
    Else
    Servo(1) = Mediumright
    Servo(2) = Medium - Progspeed
    End If
    
    Case &B00001 'Line under right sensor
    If Overshoot = 0 Then
    Servo(1) = Hardright
    Servo(2) = Mediumslow - Progspeed
    Else
    Servo(1) = Largeright
    Servo(2) = Mediumslow - Progspeed
    End If
    
    
    'Left Sensors
    
    Case &B01100 'Line between center and midleft sensor
    Overshoot = 0
    Servo(1) = Smallleft
    Servo(2) = Mediumfast - Progspeed
    
    Case &B01000 'Line under midleft sensor
    Overshoot = 0
    Servo(1) = Mediumleft
    Servo(2) = Mediumfast - Progspeed
    
    Case &B11000 'Line between midleft and left sensor
    If Overshoot = 0 Then
    Servo(1) = Largeleft
    Servo(2) = Medium - Progspeed
    Else
    Servo(1) = Center
    Servo(2) = Medium - Progspeed
    End If
    
    
    Case &B10000 'Line under left sensor
    If Overshoot = 0 Then
    Servo(1) = Hardleft
    Servo(2) = Mediumslow - Progspeed
    Else
    Servo(1) = Largeleft
    Servo(2) = Mediumslow - Progspeed
    End If
    
    
    Case &B11111 'This should never happen, probably an error. Turn in circle to find line
    Servo(1) = Largeleft
    Servo(2) = Slow - Progspeed
    
    End Select
    
    Loop
    End
    
    '****************************************
    'Subroutines
    '****************************************
    
    Sub Doadc()
    
    For X = 0 To 4 'Cycle through sensors
    ' Temp = 0
    ' For I = 0 To 2 'Take 3 samples
    ' Adcvalue = Getadc(x)
    ' Toggle Led1
    ' Temp = Temp + Adcvalue
    ' Next I
    ' Temp = Temp / 3
    ' Lastadc(x + 1) = Temp 'Average the 3 samples
    ' Adcvalue = Getadc(x)
    Lastadc(x + 1) = Getadc(x)
    
    Next X
    
    
    End Sub
    
    Sub Waitforbutton()
    
    Cont1:
    'Print "Checking Button"
    Debounce Buttonleft , 0 , Cont2
    Debounce Buttonright , 0 , Cont2
    
    Goto Cont1
    
    Cont2:
    Print "Button Pushed"
    
    End Sub
    
    
    Complete:
    X = 1
    Return
    
    Program:
    Incr Progspeed
    If Progspeed > 8 Then Progspeed = 0
    
    For I = 0 To Progspeed
    Reset Redled
    Waitms 100
    Set Redled
    Waitms 100
    Next I
    
    Return
    

    pochodzący ze strony http://www.wrighthobbies.net/guides/linefollower.htm
    Mógłby ktoś powiedzieć gdzie tu zachodzi autokalibracja i jak się odbywa. Resztę sobie skonfiguruje (zastosuje zamiast serw PWM itd.)


    Przeniesiono z Programowanie Ogólne. - arnoldziq
  • REKLAMA
REKLAMA