'********************************************************************************** ' Gallerydrive Drivecontrol Meyra Sprint '********************************************************************************** ' New added feature: Serout for GDdoctor Pro '********************************************************************************** $regfile = "m8def.dat" 'Atmega8-declarations $crystal = 3686400 $baud = 57600 On Timer0 Odometer Config Timer0 = Timer , Prescale = 8 Enable Timer0 Enable Interrupts Config Serialout = Buffered , Size = 254 Ddrb = &B00100111 Portb = &B11011000 '1= pull-up on 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 Oolprint As Byte 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 Calibrateprint As Byte 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 -------------------- '---------------------------------------------------------------------------------- '********************************************************************************** ' 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, '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, '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 '---------------------------------------; 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 '********************************************************************************** ' GDdoctor Pro communication '********************************************************************************** 'Incr Displaycount 'Displayrefresh = Displaycount Mod 8 'If Displayrefresh = 0 Then Print Chr(0); 'synchronize data stream (block of data always starts with 'nul') '********************************************************************************** ' print out bargraphs '********************************************************************************** For I = 32 To 1 Step -1 'calculating ideal values for bargraph display If Ldr(i) > 254 Then Ldr(i) = 255 'crop levels If Ldr(i) < 2 Then Ldr(i) = 1 Print Chr(ldr(i)); Next I Print Chr(linepos); 'transmit calculated lineposition Print Chr(avbright); 'average brightness Print Chr(brightval); 'value of 'brightest' sensor If Kc < 1 Then Kc = 1 If Kc > 255 Then Kc = 255 Print Chr(kc); 'value of contrast poti Print Chr(steer); 'steering value Print Chr(drive); 'driving value Odoprint = Odo If Odoprint < 1 Then Odoprint = 1 If Odoprint > 255 Then Odoprint = 255 Print Chr(odoprint); 'odometer value Oolprint = Ool + 10 Print Chr(oolprint); 'out of line Calibrateprint = Calibrate + 10 Print Chr(calibrateprint); 'calibrate If Kp < 1 Then Kp = 1 If Kd < 1 Then Kd = 1 If Ks < 1 Then Ks = 1 If Kp > 255 Then Kp = 255 If Kd > 255 Then Kd = 255 If Ks > 255 Then Ks = 255 Print Chr(kp); Ki = Ki + 1 Print Chr(ki); Print Chr(kd); Print Chr(ks); '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 ')