'********************************************************************************** ' Gallerydrive Drivecontrol Meyra Sprint '********************************************************************************** ' New added feature: Error Output '********************************************************************************** $regfile = "m8def.dat" 'Atmega8-declarations $crystal = 3686400 $baud = 57600 $eeprom On Timer0 Odometer Config Timer0 = Timer , Prescale = 64 Enable Timer0 Enable Interrupts Config Serialout = Buffered , Size = 254 'speedcontrol#1 (14%): pb4 'speedcontrol#2 (29%): pb3 'speedcontrol#3 (57%): pd0 Ddrb = &B00100111 Portb = &B11000000 '1= pull-up on Ddrc = &B11000000 Portc = &B00111111 Ddrd = &B00001110 Portd = &B11110000 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 Error As Byte 'will be !=0, if an error of the ldu occurs Dim Error_print As Byte 'variable will be sent to gddoctor pro Dim Steer As Byte 'steering voltage Dim Speed As Long 'driving voltage Dim Drive As Byte 'byte variable for transmitting speed Dim Speed_con_out As Byte 'speedcontrol outout to gddoctor 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 Alarm As Bit Dim Alarmdelay As Byte 'counter variable, which delays alarm output Dim Alarmtime As Byte Alarmtime = 50 'delay, after which the alarm is switched on, when "out of line" - increase this number if you are using a faster crystal 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 Eewhite(32) As Integer 'white calibration values, stored in eeprom Dim Eeblack(32) As Integer 'black calibration values, stored in eeprom Dim Eewhitecompare(32) As Integer 'copy of white calibration value, stored at another memory position Dim Eeblackcompare(32) As Integer 'copy of black calibration value, stored at another memory position 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 Odocalc As Long 'calculating variable for speed feedback Dim Desired_speed As Long 'speedvalue coming from entertainment controller Dim Esum_output As Long 'speed integral output to gddoctor 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 Dim E_speed As Long Dim Esum_speed As Long Const Ta = 3 Const X = 160000 Waitms 1000 '---------------------------------------------------------------------------------- '------------------ motordriver startsequence meyra sprint ------------------------ '---------------------------------------------------------------------------------- Startsequence: Odo = 250 Drive = 157 Steer = 157 Error = 0 Portb.0 = 0 'rotating blue light off For I = 1 To 240 Gosub I2c_wheelchairsend 'sends drive, steer and reference to wheelchair via i2c Portb.2 = 1 '24V high Waitms 5 Next I Portb.2 = 0 '24V low Waitms 500 For I = 1 To 240 Gosub I2c_wheelchairsend 'sends drive, steer and reference to wheelchair via i2c Portb.2 = 1 '24V high Waitms 5 Next I '---------------------------------------------------------------------------------- '------------- write stored eeprom calibration values into arrays ----------------- '---------------------------------------------------------------------------------- For I = 1 To 32 Readeeprom J , I 'black Eeblack(i) = J K = I + 32 Readeeprom J , K 'white Eewhite(i) = J K = I + 64 Readeeprom J , K Eeblackcompare(i) = J K = I + 96 Readeeprom J , K Eewhitecompare(i) = J If Eewhite(i) <> Eewhitecompare(i) Or Eeblack(i) <> Eeblackcompare(i) Then Error = 3 'error3: eeprom memory error / calibrate again Next I '********************************************************************************** '------------------------------- PROGRAMLOOP START -------------------------------- '********************************************************************************** Do If Pind.5 = 0 Then Goto Startsequence 'reset-button pushed? 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 ------------------------------- '********************************************************************************** If Pinc.5 = 0 Then Gosub Calibration 'calibrate-button pushed? Else If Calibrate > 0 Then Calibrate = 0 End If '********************************************************************************** '-------------------------------- adjust ldr-array -------------------------------- '********************************************************************************** If Calibrate <> 2 Then 'adjust black For I = 1 To 32 If Ldr(i) = 0 Or Ldr(i) = 255 Then If Error = 0 And Error <> 3 Then Error = 1 'error1: unexpected value from adc's / check electronics End If Ldr(i) = Ldr(i) - Eeblack(i) 'set blacklevel to 0 (ca.) Next I End If For I = 1 To 32 'adjust white Ldr(i) = Ldr(i) * 100 Ldr(i) = Ldr(i) / Eewhite(i) Ldr(i) = Ldr(i) + 50 'level of ldr(i) is: 50=black 150=white If Ldr(i) < 5 Or Ldr(i) > 195 Then If Error = 0 And Error <> 3 Then Error = 2 'error2: unexpected value from calibrated ldu / check calibration End If 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 If Error <> 3 Then 'if no eeprom memory occured: For I = 1 To 32 If Eewhite(i) <> Eewhitecompare(i) Or Eeblack(i) <> Eeblackcompare(i) Then Gosub Varmemerror 'memory error during loop / read values from eeprom again 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 '********************************************************************************** '------------------------ if error occured - display error ------------------------ '********************************************************************************** If Error <> 0 Then Goto Errors '********************************************************************************** '------------------------------ 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 Alarmdelay = 0 Else Ool = 1 If Alarmdelay < Alarmtime Then Alarmdelay = Alarmdelay + 1 'increase the "50" in this line and over next line, if you are using a faster crystal 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 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 '********************************************************************************** '--------------------------- calculate driving voltage --------------------------- '********************************************************************************** Ks = Ks - 42 Ks = Ks * 10 Ks = Ks / 98 'ks (speed poti) ranges now between 0 and 100 If Ks > 100 Then Ks = 100 Desired_speed = 0 If Pinb.4 = 1 Then Desired_speed = Desired_speed + 14 'speedcontrol#1 (14%): pb4 If Pinb.3 = 1 Then Desired_speed = Desired_speed + 29 'speedcontrol#2 (29%): pb3 If Pind.0 = 1 Then Desired_speed = Desired_speed + 57 'speedcontrol#3 (57%): pd0 Speed_con_out = Desired_speed 'store speedcontrol value in speed_con_out for transmission to GDdoctorPro Desired_speed = Desired_speed * Ks 'multiplies the poti value with speedcontrol value, 'received by the entertainment controller 'speedcontrol is now the desired speed, ranging between 0 and 10000 Odocalc = 250 - Odo 'odo:200=slowest speed / 10=full speed / 250=stands still Odocalc = Odocalc * 1000 Odocalc = Odocalc / 24 If Odocalc > 10000 Then Odocalc = 10000 'odocalc is the current speed, ranging between 0 and 10000 (0=no speed - 10000=full speed) E_speed = Desired_speed - Odocalc 'e_speed = difference between desired and real speed If Ool = 0 Then Esum_speed = Esum_speed + E_speed 'sum up difference to build an integral If Esum_speed < -500000 Then Esum_speed = -500000 'crop integral If Esum_speed > 500000 Then Esum_speed = 500000 Esum_output = Esum_speed Speed = Esum_speed / 400 Speed = Speed + Desired_speed 'add integral to speed. - the result will be, 'that if the wheelchair is too slow, it will increase its power 'and if it is too fast, it will slow down. Speed = Speed / 625 'change range of speed to the driving voltage Speed = Speed + 157 'output range If Speed < 157 Then Speed = 157 'crop speed to allowed driving voltages If Speed > 173 Then Speed = 173 Drive = Speed 'drive_out=bytevariable for transmitting "drive" '********************************************************************************** '---------------------------- out of line safety stop ---------------------------- '********************************************************************************** If Ool = 1 Then 'stop if there's no line Drive = 157 Steer = 157 Esum = 0 End If '********************************************************************************** '--------------------------- wheelchair communication ----------------------------- '********************************************************************************** 'set driving voltages via i2c at pin 5/6/7 of sub-d plug Gosub I2c_wheelchairsend 'sends drive, steer and reference to wheelchair via i2c '********************************************************************************** '---------------------------- Alert & Motordriver Error I/O ----------------------- '********************************************************************************** Alarm = 0 If Alarmdelay = Alarmtime And Pinc.5 = 1 Then Alarm = 1 'alert on (if car is already 5000 cycles out of line & calibrate button is not pushed) If Portd.4 = 0 Then Alarm = 1 'error signal from motordriver If Alarm = 1 Then Portb.0 = 1 'rotating blue light on Esum_speed = 0 Else Portb.0 = 0 'rotating blue light off End If '********************************************************************************** '----------------------------- GDdoctor Pro communication ------------------------- '********************************************************************************** Gddoctor: '-------------------------------------------------- ' calculating ideal transmission values '-------------------------------------------------- If Kc < 1 Then Kc = 1 If Kc > 255 Then Kc = 255 Odoprint = Odo If Odoprint < 1 Then Odoprint = 1 If Odoprint > 255 Then Odoprint = 255 Oolprint = Ool + 10 Calibrateprint = Calibrate + 10 Kp = Kp + 1 Ki = Ki + 1 Kd = Kd + 1 Ks = Ks + 1 Speed_con_out = Speed_con_out + 1 Esum_output = Esum_output / 20000 'speed integral ranges from -100000 to 5000000 Esum_output = Esum_output + 26 If Esum_output > 255 Then Esum_output = 255 If Esum_output < 1 Then Esum_output = 1 Error_print = Error_print + 1 '-------------------------------------------------- ' transmitting bargraph values '-------------------------------------------------- Print Chr(0); 'synchronize data stream (block of data always starts with 'nul' ) For I = 32 To 1 Step -1 'transmitting bargraph values Print Chr(ldr(i)); Next I '-------------------------------------------------- ' transmitting other values '-------------------------------------------------- Print Chr(linepos); Print Chr(avbright); Print Chr(brightval); Print Chr(kc); Print Chr(steer); Print Chr(drive); Print Chr(odoprint); Print Chr(oolprint); Print Chr(calibrateprint); Print Chr(kp); Print Chr(ki); Print Chr(kd); Print Chr(ks); Print Chr(speed_con_out); Print Chr(esum_output); Print Chr(error_print); Error_print = Error_print - 1 Loop End '********************************************************************************** '----------------------------------- LOOP END ------------------------------------- ' '--------------------------- SUBROUTINES START BELOW ------------------------------ '********************************************************************************** '---------------------------------------------------------------------------------- '------------------------------- calibrate ldr-array ------------------------------ '---------------------------------------------------------------------------------- '-------------------------- ALWAYS CALIBRATE BLACK FIRST! ------------------------- '---------------------------------------------------------------------------------- Calibration: 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. K = I + 64 Writeeeprom J , K 'write value at another proition for safety Eeblack(i) = J Eeblackcompare(i) = J 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 Ldr(i) = Ldr(i) - Eeblack(i) If Ldr(i) < 0 Then Ldr(i) = 0 If Ldr(i) > 255 Then Ldr(i) = 255 J = Ldr(i) ' values of the white ldr's in the eeprom K = I + 32 ' (address 33-64), then. Writeeeprom J , K K = K + 64 Writeeeprom J , K Eewhite(i) = J Eewhitecompare(i) = J Next I End If '--------------------------------------- ' check stored values '--------------------------------------- Gosub Varmemerror Return '---------------------------------------------------------------------------------- '-------------------------------- display errors ---------------------------------- '---------------------------------------------------------------------------------- Errors: 'stop car first! Drive = 157 Steer = 157 Gosub I2c_wheelchairsend 'sends drive, steer and reference to wheelchair via i2c 'errors are displayed on the rotating blue light 'the blue light blinks quickly for several times (=errorcode #) 'after that, it is switched on for a bit longer. ' 'table of errorcodes: '-------------------- 'error1: unexpected value from adc's (0 or 255) / check electronics 'error2: unexpected value from calibrated ldu (too low or too high) / check calibration (this could perhaps also be a reflecting floor?) 'error3: eeprom memory error (both stored calibration values in the eeprom do not match) / calibrate again For I = 1 To Error Portb.0 = 1 'rotating blue light on Waitms 300 Portb.0 = 0 Waitms 400 Next I Portb.0 = 1 'rotating blue light on Waitms 1500 Portb.0 = 0 Waitms 600 Error_print = Error Error = 0 Goto Gddoctor '---------------------------------------------------------------------------------- '------ repairing variable memory error by reading values again from eeprom ------- '---------------------------------------------------------------------------------- Varmemerror: For I = 1 To 32 Readeeprom J , I 'black Eeblack(i) = J K = I + 32 Readeeprom J , K 'white Eewhite(i) = J K = I + 64 Readeeprom J , K Eeblackcompare(i) = J K = I + 96 Readeeprom J , K Eewhitecompare(i) = J If Eewhite(i) <> Eewhitecompare(i) Or Eeblack(i) <> Eeblackcompare(i) Then Error = 3 'error3: eeprom memory error / calibrate again Next I Return '---------------------------------------------------------------------------------- '-------------- sending control voltages to wheelchair via i2c -------------------- '---------------------------------------------------------------------------------- I2c_wheelchairsend: If Drive = 157 Then Steer = 157 'don't steer, if you don't drive If Drive <> 157 Or Steer <> 157 Then Portb.1 = 1 Else Portb.1 = 0 'set totmann switch if driving = yes or steering = yes '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 Return '---------------------------------------------------------------------------------- '-- odointerrupt - returns "odo" 10=full speed 200=minimal speed 250=no speed -- '---------------------------------------------------------------------------------- Odometer: If Odocount < 250 Then Odocount = Odocount + 1 Else Odo = 250 'increment odocount Odonew = Pind.7 'pind.7= odosensor (0=marker at wheel - the wheel has two markers) If Odoold = 1 And Odonew = 0 Then 'if a marker was detected, and if there was no marker at the last interrupt: Odo = Odocount 'write counted value into odo Odocount = 0 'reset counter variable End If Odoold = Odonew 'remember last state of odo sensor Return '----------------------------------------------------------------------------------