'********************************************************************************** ' Gallerydrive Drivecontrol Meyra Sprint '********************************************************************************** ' '********************************************************************************** $regfile = "m8def.dat" 'Atmega8-declarations $crystal = 3686400 $baud = 9600 On Timer0 Odometer Config Timer0 = Timer , Prescale = 8 Enable Timer0 Enable Interrupts Config Serialout = Buffered , Size = 255 Ddrb = &B00100111 Portb = &B11011000 Ddrc = &B11000000 Portc = &B00111111 Ddrd = &B00001110 Portd = &B11110001 Config Sda = Portd.2 Config Scl = Portd.3 Config I2cdelay = 10 Config Adc = Single , Prescaler = Auto , Reference = Internal Start Adc Dim Odonew As Bit 'current odometer state Dim Odoold As Bit 'previous odometer state Dim Ool As Bit '0=on line 1=out of line Dim Displayrefresh As Byte 'takes care, that data to display is just sent every XX program cycle Dim Displaycount As Byte Dim Linepos As Byte 'lineposition Dim Brightval As Byte 'value of brightest sensor Dim Avbright As Integer 'average brightnes of sensors which are not *the* brightest sensor Dim Steer As Byte 'steering voltage Dim Drive As Byte 'driving voltage Dim I As Byte 'i...m = general purpose variables Dim J As Byte Dim K As Byte Dim L As Byte Dim M As Integer Dim Calibrate As Byte 'stores values for the state of calibrating the ldr-array Dim Ldr(32) As Integer 'ldr contains the value of the light dependant resistors (int:-32,768 to +32,767) Dim Kp As Integer 'verstärkungsfaktor potential-glied Dim Ki As Integer 'verstärkungsfaktor integral-glied Dim Kd As Integer 'verstärkungsfaktor differential-glied Dim Ks As Integer 'speedpoti Dim Kc As Integer 'contrastpoti Dim Odoprint As Word 'odovalue for bargraph Dim Odo As Word 'odometer Dim Odocount As Word 'counter for odometer Dim Odosmooth As Word Dim E As Long 'variables & constants for PID-control Dim Esum As Long Dim S1 As Long Dim S2 As Long Dim S3 As Long Dim W As Long Dim Y As Long Dim Ealt As Long Const Ta = 3 Const X = 160000 Waitms 1000 '---------------------------------------------------------------------------------- '------------------ motordriver startsequence meyra sprint ------------------------ '---------------------------------------------------------------------------------- Drive = 157 Steer = 157 For I = 1 To 240 'reference: I2cstart 'i2c start condition I2cwbyte &B10010100 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on I2cwbyte 157 '157=6.0V I2cstop 'i2c stop condition 'steering: I2cstart 'i2c start condition I2cwbyte &B10010110 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on I2cwbyte 157 '139=5.3V | 157=6.0V | 176=5.7V I2cstop 'i2c stop condition 'driving: I2cstart 'i2c start condition I2cwbyte &B10010000 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on I2cwbyte 157 '139=5.3V | 157=6.0V | 176=5.7V I2cstop 'i2c stop condition Portb.2 = 1 '24V high Waitms 5 Next I Portb.2 = 0 '24V low Waitms 500 For I = 1 To 240 'reference: I2cstart 'i2c start condition I2cwbyte &B10010100 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on I2cwbyte 157 '157=6.0V I2cstop 'i2c stop condition 'steering: I2cstart 'i2c start condition I2cwbyte &B10010110 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on I2cwbyte 157 '139=5.3V | 157=6.0V | 176=5.7V I2cstop 'i2c stop condition 'driving: I2cstart 'i2c start condition I2cwbyte &B10010000 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on I2cwbyte 157 '139=5.3V | 157=6.0V | 176=5.7V I2cstop 'i2c stop condition Portb.2 = 1 '24V high Waitms 5 Next I '---------------------------------------------------------------------------------- '--------------- end of motordriver startsequence meyra sprint -------------------- '---------------------------------------------------------------------------------- '********************************************************************************** ' initialize terminal display (tvt-mbkd-11) '********************************************************************************** Print Chr(27) ; "[?15h" ; Waitms 20 Print Chr(27) ; "[>5h" ; Waitms 20 Print Chr(27) ; "[?16h" ; Waitms 20 Print Chr(27) ; "[2J" ; Waitms 20 Print Chr(27) ; "[=h" ; Waitms 40 Print "~1 4111111110000000011111111000000001111111100000000111111110000000011001100~"; Waitms 40 Print "7 ADC8 ADC7 ADC6 ADC5 ADC4 ADC3 ADC2 ADC1 RSDO~6 LINE NO LINE~5 LINESCAN DEBUG MODE~3"; '********************************************************************************** ' programloop starts here '********************************************************************************** Do Portb.5 = Not Portb.5 'status led blinks Kp = Getadc(0) 'readout potis Ki = Getadc(1) Kd = Getadc(2) Ks = Getadc(3) Kc = Getadc(4) '********************************************************************************** ' read the values of the 32 sensors via i²c bright: ca.30 dark: ca.230 '********************************************************************************** I2cstart 'i2c start condition I2cwbyte &B10010000 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on) I2cstart 'repeated start condition I2cwbyte &B10010001 'send pcf addressieren (read) I2crbyte Ldr(1) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(2) , Ack '... I2crbyte Ldr(3) , Ack '... I2crbyte Ldr(4) , Ack 'read value of adc3 / don't acknowledge I2crbyte Ldr(1) , Nack I2cstop I2cstart 'i2c start condition I2cwbyte &B10010010 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on) I2cstart 'repeated start condition I2cwbyte &B10010011 'send pcf addressieren (read) I2crbyte Ldr(5) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(6) , Ack '... I2crbyte Ldr(7) , Ack '... I2crbyte Ldr(8) , Ack 'read value of adc3 / don't acknowledge I2crbyte Ldr(5) , Nack I2cstop I2cstart 'i2c start condition I2cwbyte &B10010100 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on) I2cstart 'repeated start condition I2cwbyte &B10010101 'send pcf addressieren (read) I2crbyte Ldr(9) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(10) , Ack '... I2crbyte Ldr(11) , Ack '... I2crbyte Ldr(12) , Ack 'read value of adc3 / don't acknowledge I2crbyte Ldr(9) , Nack I2cstop I2cstart 'i2c start condition I2cwbyte &B10010110 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on) I2cstart 'repeated start condition I2cwbyte &B10010111 'send pcf addressieren (read) I2crbyte Ldr(13) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(14) , Ack '... I2crbyte Ldr(15) , Ack '... I2crbyte Ldr(16) , Ack 'read value of adc3 / don't acknowledge I2crbyte Ldr(13) , Nack I2cstop 'i2c stop condition I2cstart 'i2c start condition I2cwbyte &B10011000 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on) I2cstart 'repeated start condition I2cwbyte &B10011001 'send pcf addressieren (read) I2crbyte Ldr(17) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(18) , Ack '... I2crbyte Ldr(19) , Ack '... I2crbyte Ldr(20) , Ack 'read value of adc3 / don't acknowledge I2crbyte Ldr(17) , Nack I2cstop 'i2c stop condition I2cstart 'i2c start condition I2cwbyte &B10011010 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on) I2cstart 'repeated start condition I2cwbyte &B10011011 'send pcf addressieren (read) I2crbyte Ldr(21) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(22) , Ack '... I2crbyte Ldr(23) , Ack '... I2crbyte Ldr(24) , Ack 'read value of adc3 / don't acknowledge I2crbyte Ldr(21) , Nack I2cstop 'i2c stop condition I2cstart 'i2c start condition I2cwbyte &B10011100 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on) I2cstart 'repeated start condition I2cwbyte &B10011101 'send pcf addressieren (read) I2crbyte Ldr(25) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(26) , Ack '... I2crbyte Ldr(27) , Ack '... I2crbyte Ldr(28) , Ack 'read value of adc3 / don't acknowledge I2crbyte Ldr(25) , Nack I2cstop 'i2c stop condition I2cstart 'i2c start condition I2cwbyte &B10011110 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on) I2cstart 'repeated start condition I2cwbyte &B10011111 'send pcf addressieren (read) I2crbyte Ldr(29) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(30) , Ack '... I2crbyte Ldr(31) , Ack '... I2crbyte Ldr(32) , Ack 'read value of adc3 / don't acknowledge I2crbyte Ldr(29) , Nack I2cstop 'i2c stop condition '********************************************************************************** ' calibrate ldr-array ' ' always calibrate black first!!! '********************************************************************************** If Pinc.5 = 0 Then 'calibrate-button pushed? If Pind.6 = 1 Then 'linecolor-switch 0=white 1=black '--------------------------------------- ' calibrate black '--------------------------------------- ' when the >calibrate<-button is pushed Calibrate = 1 ' and the >linecolor<-switch is on black, Print "~5 CALIBRATE BLACK ~3"; 'you have to put something black under For I = 1 To 32 ' the line detector. this code stores all J = Ldr(i) ' values of the black ldr's in the eeprom Writeeeprom J , I ' (address 1-32), then. Next I Else '--------------------------------------- ' calibrate white '--------------------------------------- ' when the >calibrate<-button is pushed Calibrate = 2 ' and the >linecolor<-switch is on white, Print "~5 CALIBRATE WHITE ~3"; 'you have to put something white under For I = 1 To 32 ' the line detector. this code stores all Readeeprom J , I Ldr(i) = Ldr(i) - J J = Ldr(i) ' values of the white ldr's in the eeprom K = I + 32 ' (address 33-64), then. Writeeeprom J , K Next I End If Else If Calibrate > 0 Then '--------------------------------------- ' reset display if calibrate is over '--------------------------------------- Print "~5 LINESCAN DEBUG MODE ~3"; Calibrate = 0 End If End If '********************************************************************************** ' adjust ldr-array '********************************************************************************** If Calibrate <> 2 Then 'adjust black For I = 1 To 32 Readeeprom J , I Ldr(i) = Ldr(i) - J Next I End If For I = 1 To 32 'adjust white K = I + 32 Readeeprom J , K Ldr(i) = Ldr(i) * 100 Ldr(i) = Ldr(i) / J Ldr(i) = Ldr(i) + 50 'level of ldr(i) is: 50=black 150=white Next I If Pind.6 = 1 Then 'switch linecolor according to switch For I = 1 To 32 Ldr(i) = 200 - Ldr(i) Next I End If '********************************************************************************** ' calculate lineposition and write it into var:linepos '********************************************************************************** Linepos = 1 Brightval = Ldr(1) For I = 2 To 32 'find out brightest sensor If Ldr(i) > Brightval Then Linepos = I Brightval = Ldr(i) End If Next I '********************************************************************************** ' detect out of line '********************************************************************************** L = 0 Avbright = 0 For I = 1 To 32 'detect out of line J = I - 1 K = I + 1 If J = 0 Then J = 1 If K = 33 Then K = 32 If I <> Linepos And J <> Linepos And K <> Lineposthen Avbright = Avbright + Ldr(i) L = L + 1 End If Next I Avbright = Avbright / L Kc = Kc - 42 'calculate good value area for contrast poti (0-49) Kc = Kc / 20 M = Brightval - Kc If M >= Avbright Then Ool = 0 Portb.0 = 0 'alert off Else Ool = 1 If Pinc.5 = 1 Then Portb.0 = 1 'alert on (if out of line & calibrate button is not pushed) End If '********************************************************************************** ' PID regulation steering voltage '********************************************************************************** 'W: Sollwert 'X: Istwert 'E: Regelabweichung 'Y: Stellgröße 'Esum: Summe der Regelabweichungen 'Ealt: Regelabweichung im vorherigen Programmdurchlauf 'Ta: Zeit 's1-s4: temporary variables Kp = Kp - 42 Kp = Kp / 10 Ki = Ki - 42 Ki = Ki / 100 Kd = Kd - 42 Kd = Kd / 5 W = Linepos * 10000 ' PID-code: ' /-----------------\ E = W - X Esum = Esum + E If Esum < -5000000 Then Esum = -5000000 'crop i at maxima If Esum > 5000000 Then Esum = 5000000 S1 = Kp * E S2 = Ki * Esum S2 = S2 / 10 S3 = E - Ealt S3 = S3 * Kd Y = S1 + S2 Y = Y + S3 ' \-----------------/ Y = Y / 10000 'y goes from -2000 to 2000 (ca.) Y = -y Ealt = E '********************************************************************************** '--------------------------- calculate driving voltage --------------------------- '********************************************************************************** Y = Y / 55 Y = Y + 158 If Y < 139 Then Y = 139 'crop y to allowed steering voltages If Y > 173 Then Y = 173 Steer = Y Ks = Ks - 42 Ks = Ks / 55 Drive = Ks + 157 If Drive < 157 Then Drive = 157 'crop drive to allowed voltages If Drive > 173 Then Drive = 173 If Ool = 1 Then 'stop if there's no line Drive = 157 Steer = 157 Esum = 0 End If '********************************************************************************** '--------------------------- wheelchair communication ----------------------------- '********************************************************************************** If Drive <> 157 Or Steer <> 157 Then Portb.1 = 1 Else Portb.1 = 0 'set totmann switch if driving = yes or steering = yes 'set driving voltages via i2c at pin 5/6/7 of sub-d plug 'reference: I2cstart 'i2c start condition I2cwbyte &B10010100 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on I2cwbyte 157 '157=6.0V I2cstop 'i2c stop condition 'driving: I2cstart 'i2c start condition I2cwbyte &B10010000 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on I2cwbyte Drive '139=5.3V | 158=6.0V | 176=5.7V I2cstop 'i2c stop condition 'steering: I2cstart 'i2c start condition I2cwbyte &B10010110 'send pcf address (write) I2cwbyte &B01000101 'send control byte (auto-increment on, internal oscillator on I2cwbyte Steer '139=5.3V | 158=6.0V | 176=5.7V I2cstop 'i2c stop condition '********************************************************************************** ' odometer odo:1000=no speed / 0=full speed '********************************************************************************** Odo = Odo * 2 Odo = Odo + Odosmooth Odo = Odo / 3 '********************************************************************************** ' print out line position, average brightnes, line brightnes, contrast '********************************************************************************** Displayrefresh = Displaycount Mod 8 If Displayrefresh = 0 And Calibrate = 0 Then 'display values for debugging If Ool = 0 Then Print "~5LP: " ; Linepos ; 'LP: lineposition If Linepos < 10 Then Print " "; Else Print "~5NOLINE"; End If Print " A: " ; Avbright ; 'A: average brightnes If Avbright < 100 Then Print " "; Print " B: " ; Brightval ; 'B: value of 'brightest' sensor If Brightval < 100 Then Print " "; Print " C: " ; Kc ; " ~3"; 'C: value of contrast poti End If '********************************************************************************** ' print out bargraphs '********************************************************************************** Incr Displaycount Displayrefresh = Displaycount Mod 2 If Displayrefresh = 0 Then 'sending data to terminal every 3rd program cycle For I = 32 To 1 Step -1 'calculating ideal values for bargraph display Ldr(i) = Ldr(i) * 2 Ldr(i) = Ldr(i) / 3 Ldr(i) = Ldr(i) + 15 If Ldr(i) > 123 Then Ldr(i) = 123 'crop levels If Ldr(i) < 32 Then Ldr(i) = 32 Print Chr(ldr(i)); Next I Printbin 77; 'print reference bargraph steer/drive Steer = Steer - 139 'calculate steer bargraph Steer = Steer * 2 Steer = Steer + 41 Printbin Steer; Drive = Drive - 139 'calculate drive bargraph Drive = Drive * 2 Drive = Drive + 41 Printbin Drive; Odoprint = Odo / 12 'calculate odo bargraph Odoprint = 84 - Odoprint Odoprint = Odoprint + 32 Printbin Odoprint; Print "~3"; End If Loop End '********************************************************************************** ' odointerrupt '********************************************************************************** Odometer: Odonew = Pind.7 If Odocount < 1000 Then Odocount = Odocount + 1 'Else Odo = 0 'too slow:odo=0 If Odonew <> Odoold And Odonew = 0 And Odocount > 50 Then 'hole at odowheel Odosmooth = Odocount Odocount = 0 End If If Odocount > Odosmooth Then Odosmooth = Odocount Odoold = Odonew Return '( currently unused i/o ports... If Pind.5 = 0 Then Waitms 1000 'reset-button pushed? If Pind.6 = 0 Then Waitms 1000 'linecolor-switch 0=white 1=black Portb.0 = Not Portb.0 'alert out Kp = Kp / 5 'values between 8 and 204 Ki = Ki / 5 Kd = Kd / 5 Ks = Ks / 5 Kc = Kc / 5 ')