'----------------------------------------------------------------------------------- '- meyra "sprint" pid control - '- - '- connected devices: - '- - '- adc0: poti (p) - '- adc1: poti (i) - '- adc2: poti (d) - '- adc3: poti (speed) - '- - '- portb.0: piezo speaker for sonic alert (out of line) - '- portb.1: relais for totmann switch - '- portb.2: relais for switching 24V - '- - '- portd.4: sda || i²c communication with line detection unit and dac's for - '- portd.5: scl || generating the driving, steering and reference voltage - '- - '----------------------------------------------------------------------------------- $regfile = "m8def.dat" 'Atmega8-declarations $crystal = 3686400 $baud = 9600 Ddrb = &B00000111 Ddrc = &B00000000 Portb = &B00000000 Config Sda = Portd.5 Config Scl = Portd.4 Config I2cdelay = 10 Config Adc = Single , Prescaler = Auto , Reference = Internal Start Adc Const Contrast = 35 'adjust contrast between surface and line (ca.20...50 20=no contrast 50=high contrast -- this contrastlevel is needed to find out, whether there's a line or not -- if there's no line, an alert will occur) Dim I As Byte 'i-l: general counter variables Dim J As Byte Dim K As Byte Dim L As Byte Dim Speed As Byte 'speed Dim Steer As Byte 'steer Dim Steerfactor As Byte 'absolute value of steer Dim Wp As Word 'poti readouts Dim Wi As Word Dim Ws As Word Dim Ldr(16) As Integer 'ldr contains the value of the light dependant resistors Dim Maxbrightval As Byte 'maxbright=value of brightest ldr Dim Maxbrightsen As Byte 'maxbrightsensor=number of brightest ldr Dim Difbright As Word 'difference between the brightest sensor and the average brightnes of the other sensors Dim Ool As Byte 'outofline alert 'pid-control Dim Y As Long 'stellwert Dim Yalt As Long Dim Kp As Long 'verstärkungsfaktor potential-glied Dim Ki As Long 'verstärkungsfaktor integral-glied Dim Kd As Long 'verstärkungsfaktor differential-glied Dim Ealt As Long 'regelabweichung aus letztem durchlauf Dim E As Long 'aktuelle regelabweichung Dim Esum As Long 'summe aller bisherigen regelabweichungen Dim S1 As Long 'general calculating variables Dim S2 As Long Dim S3 As Long Dim S4 As Long Do Speed = 157 Steer = 157 '----------------------------------------------------------------------------------- '------------------- motordriver startsequence meyra sprint ------------------------ '----------------------------------------------------------------------------------- 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 -------------------- '----------------------------------------------------------------------------------- Drive: '----------------------------------------------------------------------------------- '---------------------------------- read out ldu ---------------------------------- '----------------------------------------------------------------------------------- ' i read out all adc's twice, because the first four values seem to be stored values. ' so, just the second group of values will be used. 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) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(2) , Ack '... I2crbyte Ldr(3) , Ack '... I2crbyte Ldr(4) , Nack 'read value of adc3 / don't acknowledge 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) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(6) , Ack '... I2crbyte Ldr(7) , Ack '... I2crbyte Ldr(8) , Nack 'read value of adc3 / don't acknowledge 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) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(10) , Ack '... I2crbyte Ldr(11) , Ack '... I2crbyte Ldr(12) , Nack 'read value of adc3 / don't acknowledge 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) , Ack 'read value of adc0 & acknowledge I2crbyte Ldr(14) , Ack '... I2crbyte Ldr(15) , Ack '... I2crbyte Ldr(16) , Nack 'read value of adc3 / don't acknowledge I2cstop 'i2c stop condition ' paste code from LDU_adjust_black.bas here: Ldr(1) = Ldr(1) - 38 Ldr(2) = Ldr(2) - 37 Ldr(3) = Ldr(3) - 37 Ldr(4) = Ldr(4) - 43 Ldr(5) = Ldr(5) - 40 Ldr(6) = Ldr(6) - 40 Ldr(7) = Ldr(7) - 48 Ldr(8) = Ldr(8) - 36 Ldr(9) = Ldr(9) - 42 Ldr(10) = Ldr(10) - 40 Ldr(11) = Ldr(11) - 39 Ldr(12) = Ldr(12) - 41 Ldr(13) = Ldr(13) - 33 Ldr(14) = Ldr(14) - 33 Ldr(15) = Ldr(15) - 28 Ldr(16) = Ldr(16) - 22 ' end code from LDU_adjust_black.bas For I = 1 To 16 Ldr(i) = Ldr(i) * 200 Next I ' paste code from LDU_adjust_white.bas here: Ldr(1) = Ldr(1) / 139 Ldr(2) = Ldr(2) / 139 Ldr(3) = Ldr(3) / 142 Ldr(4) = Ldr(4) / 138 Ldr(5) = Ldr(5) / 138 Ldr(6) = Ldr(6) / 142 Ldr(7) = Ldr(7) / 140 Ldr(8) = Ldr(8) / 142 Ldr(9) = Ldr(9) / 142 Ldr(10) = Ldr(10) / 139 Ldr(11) = Ldr(11) / 141 Ldr(12) = Ldr(12) / 138 Ldr(13) = Ldr(13) / 149 Ldr(14) = Ldr(14) / 148 Ldr(15) = Ldr(15) / 150 Ldr(16) = Ldr(16) / 142 ' end code from LDU_adjust_white.bas ' invert values (only if you use a black line) For I = 1 To 16 Ldr(i) = 200 - Ldr(i) Next I '----------------------------------------------------------------------------------- '----------------- detect line and write position into 'maxbrightsen" -------------- '----------------------------------------------------------------------------------- Ool = 0 'alert variables for an out of line error Maxbrightval = Ldr(1) 'find lineposition Maxbrightsen = 1 'by detecting the highest value For I = 2 To 16 'or at least one of the highest values If Ldr(i) > Maxbrightval Then Maxbrightval = Ldr(i) Maxbrightsen = I End If Next I 'out of line detection: For I = 1 To 16 'calculate difference between the brightnes of the line and the average value of the other sensors (except the both sensors, next to the one, where the line is) If Maxbrightsen = 1 Or Maxbrightsen = 16 Then L = 14 Else L = 13 If Maxbrightsen > 1 Then J = Maxbrightsen - 1 Else J = 1 If Maxbrightsen < 16 Then K = Maxbrightsen + 1 Else K = 16 If I <> Maxbrightsen And I <> J And I <> K Then Difbright = Difbright + Ldr(i) Next I Difbright = Difbright / L Difbright = Maxbrightval - Difbright 'difbright=difference between the average brightnes and the line-brightnes If Difbright < Contrast Then Ool = 1 'out of line alert, if the contrast between line and rest of surface is too low If Ool = 1 Then 'sonic alert, if there's no line, or if the line is at the edge of the sensor array Portb.0 = Not Portb.0 End If '----------------------------------------------------------------------------------- '------------------------ PID regulation steering voltage -------------------------- '----------------------------------------------------------------------------------- Kp = Getadc(0) 'read out p-level Ki = Getadc(1) 'read out i-level Kd = Getadc(2) 'read out d-level Kp = Kp / 200 Ki = Ki / 200 Kd = Kd / 20 E = Maxbrightsen 'e(error) = difference between E = E * 1000 'wanted and current value E = 8000 - E If Ool = 0 Then Esum = Esum + E 'just integrate, if wheelchair is on line S4 = S3 S1 = Kp * E S2 = Esum / 500 S2 = Ki * S2 S3 = E - Ealt S3 = S3 * Kd S3 = S3 + S4 S3 = S3 / 2 Y = S1 + S2 Y = Y + S3 Ealt = E Y = Y / 1000 Steerfactor = Abs(y) 'steerfactor gets higher, if the chair steers a lot / is 0, if the chair goes straight Y = Y + 157 Y = Y + Yalt 'smoothen y-value Y = Y / 2 Yalt = Y If Y < 142 Then Y = 142 'crop y If Y > 173 Then Y = 173 Steer = Y If Ool = 1 Then Steer = 157 'stop, if there's no line '----------------------------------------------------------------------------------- '--------------------------- calculate driving voltage --------------------------- '----------------------------------------------------------------------------------- Ws = Getadc(3) 'read out poti on adc2 (drivespeed) Steerfactor = Steerfactor / 4 'divide steerfactor to get it smaller If Steerfactor > 5 Then Steerfactor = 5 'crop steerfactor Steerfactor = 15 - Steerfactor 'invert steerfactor Ws = Ws * Steerfactor 'multiply speed with inverted steerfactor - now, the wheelchair gets slower, if it has to steer a lot and faster, when it is going straight Ws = Ws / 1000 'calc. drive speed Ws = Ws + 157 'set level for drivespeed Speed = Ws If Speed < 157 Then Speed = 157 'crop speed to allowed voltages If Speed > 173 Then Speed = 173 If Ool = 1 Then Speed = 157 'stop if there's no line '----------------------------------------------------------------------------------- '--------------------------- wheelchair communication ----------------------------- '----------------------------------------------------------------------------------- If Speed <> 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 Speed '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 Goto Drive Loop End