'ToDo 'Sht11 'verschil tussen remtijd en opterktijd 'meer stappen in instellingen 'Save calibrate settings $regfile = "M32DEF.DAT" '$lib "mcsbyteint.lbx" '$lib "datetime.lib" $crystal = 8000000 $baud = 2400 'Conditional compilation for debug use!! Const SensorsOn = 0 'If set to 1 Enable Sensors Const BumperOn = 0 'If set to 1 Enable Bumper Const MowMotorOn = 0 'If set to 1 Enable MowMotor Const RelaisOn = 1 'If set to 1 Enable 12V power Const PwmOn = 1 'If set to 1 Enable PWM interrupts '############# Disable JTAG for full PortC usage ########### 'The M323 has the JTAG enabled by default so you can not use 'Pins PORTC.2-PORTC.5 'Disable JTAG or program the fuse bit Mcusr = &H80 Mcusr = &H80 Mutekey Alias 13 Txtkey Alias 60 Upkey Alias 80 Downkey Alias 81 Rightkey Alias 86 Leftkey Alias 85 Enterkey Alias 87 Powerkey Alias 12 Menukey Alias 82 Okkey Alias 87 Selectkey Alias 107 Smartkey Alias 108 Keykey Alias 109 Sleepkey Alias 110 Norc5 Alias 255 Sht11_slk Alias Porta.0 Sht11_data Alias Porta.1 I2c_scl Alias Porta.2 'Pin 38 I2c_sda Alias Porta.3 'Pin 37 Ir_in Alias Porta.4 'Pin 36 Bumper_in Alias Porta.5 'Pin 35 Port_a6 Alias Porta.6 'Pin 34 Port_a7 Alias Porta.7 'Pin 33 Lcd_db7 Alias Portb.0 'Pin 1 Lcd_db6 Alias Portb.1 'Pin 2 Mowmotor_int2 Alias Portb.2 'Pin 3 Lcd_db4 Alias Portb.3 'Pin 4 Lcd_e Alias Portb.4 'Pin 5 Lcd_rs Alias Portb.5 'Pin 6 Miso Alias Portb.6 'Pin 7 Sck Alias Portb.7 'Pin 8 Dir_left Alias Portc.0 'Pin 22 Dir_right Alias Portc.1 'Pin 23 Pwm_left Alias Portc.2 'Pin 24 Pwm_right Alias Portc.3 'Pin 25 Port_c3 Alias Portc.4 'Pin 26 Port_c4 Alias Portc.5 'Pin 27 Mowmotor_out Alias Portc.6 'Pin 28 Lcd_db5 Alias Portc.7 'Pin 29 Lt433_rxd Alias Portd.0 'Pin 14 Lt433_txd Alias Portd.1 'Pin 15 Leftmotor_int0 Alias Portd.2 'Pin 16 Rightmotor_int1 Alias Portd.3 'Pin 17 12v_en Alias Portd.4 'Pin 18 Lt433_power Alias Portd.5 'Pin 19 Lt433_en Alias Portd.6 'Pin 20 Port_d7 Alias Portd.7 'Pin 21 '############### Set data direction of ports ############### DDRA = &B10000101 Ddrb = &B00111111 Ddrc = &B11111111 Ddrd = &B11110010 '####################### Configure LCD ##################### 'PIN mode with 4-bit Config Lcdpin = Pin , Db4 = Lcd_db4 , Db5 = Lcd_db5 , Db6 = Lcd_db6 , Db7 = Lcd_db7 , E = Lcd_e , Rs = Lcd_rs Config Lcd = 20 * 4 'Configure lcd screen Deflcdchar 1 , 16 , 32 , 15 , 8 , 8 , 8 , 15 , 32 ' Degree Celcius CHR(1) '####################### Configure I2C ##################### 'configure the used port pin for I2C Config I2cdelay = 1 ' Fast Mode Config Sda = I2c_sda Config Scl = I2c_scl '####################### Configure IR remote ############### Config Rc5 = Pina.4 'Timer2 used for timer interrupt at 8 Mhz (8E6/256/1) = 30Khz 'This Timer is used for Motor PWM Config Timer2 = Timer , Async = Off , Prescale = 1 'Timer1 used for timer interrupt at 8 Mhz (8E6/65536/64) = 2 Hz 'This Timer is used for Direction en SensorCheck Config Timer1 = Timer , Prescale = 64 Const Forwarddir = &B00000011 '3 Const Reversedir = &B00000000 '0 Const Leftdir = &B00000010 '2 Const Rightdir = &B00000001 '1 Const Pcf8591write = &B10010000 'PCF8591 write addr. Const Pcf8591read = &B10010001 ' " read " Const Slow = 0 Const Slowstop = 0 Const Fast = 1 Const Faststop = 0 Const False = 0 Const True = 1 Const LowLevel = 0 Const HiLevel = 1 Const Eramemptymask = 255 'If Eprom is empty it contains 255 Const Eramfilledmask = 170 'If Eprom is filled it contains 170 '####################### Configure Watchdog to reset ############### Config Watchdog = 16 'reset after 16 mSec used for reboot '####################### Configure INTERRUPT trigger for compiler < 1.11.7.4 ############### 'Config Int0 = Falling 'Config Int1 = Falling 'Config Int2 = Falling 'Globals PWM contoller Dim Pwmclk As Byte Dim Leftpower As Byte Dim Rightpower As Byte Dim Mowpower As Byte 'Globals 2Hz Timertick set direction en check sensors Dim Direction As Byte Dim Tmpportc As Byte Dim Disablesensors As Byte 'Globals I2C realtime clock Dim Rtcarray(6) As Byte At &H80 Dim Rtccount As Byte Dim I2cbusbusy As Byte 'These are pointers to RTCarray() for simple handling. Dim Seconde As Byte At &H80 Overlay Dim Minute As Byte At &H81 Overlay Dim Hour As Byte At &H82 Overlay Dim Day As Byte At &H83 Overlay Dim Month As Byte At &H84 Overlay Dim Year As Byte At &H85 Overlay Dim Rtcseconde As Byte Dim Rtcminute As Byte Dim Rtchour As Byte Dim Rtcday As Byte Dim Rtcmonth As Byte Dim Rtcyear As Byte 'Globals Hall Sensors Dim Mowrpmcounter As Word 'Rpm of Mowmotor Dim Prevtimemowcounter As Long Dim Leftrpm As Long 'Rpm of Leftmotor Dim Rightrpm As Long 'Rpm of Rightmotor 'Globals MenuChoices Dim Mainmenuchoice As Byte Dim DebugMode As Byte 'Globals Detection Dim Leftrfdetect As Byte Dim Midrfdetect As Byte Dim Rightrfdetect As Byte Dim Bumperdetect As Byte 'Globals UDO parameters copy from Eram Dim Mowtime As Byte Dim Losttime As Byte Dim Turntime As Byte 'Turn time for motors Dim Revtime As Byte 'Reverse time for motors Dim AcctimeUp As Byte 'Accelarate time for motors slowmode Dim AcctimeDown As Byte 'Deaccelarate time for motors slowmode Dim Maxdrivepower As Byte 'Max power for driving motors Dim Maxmowpower As Byte 'Mow disk power Dim Leftrftrig As Byte Dim Midrftrig As Byte Dim Rightrftrig As Byte 'EEprom Globals Dim Eramempty As Eram Byte '255 = empty 170 = filled Dim E_mowtime As Eram Byte Dim E_losttime As Eram Byte Dim E_turntime As Eram Byte 'Turn time for motors Dim E_revtime As Eram Byte 'Reverse time for motors Dim E_acctimeUp As Eram Byte 'Accelarate time for motors slowmode Dim E_acctimeDown As Eram Byte 'Deacelarate time for motors slowmode Dim E_maxdrivepower As Eram Byte 'Max power for driving motors Dim E_maxmowpower As Eram Byte 'Mow disk power Dim E_leftrftrig As Eram Byte Dim E_midrftrig As Eram Byte Dim E_rightrftrig As Eram Byte Dim Eparsebyte As Byte 'Special Byte to parse internal byte to Eram 'Time calculations Globals Dim Systemstarttimestampsec As Long Dim Systemruntimesec As Long Dim Systemruntimemin As Long Dim Losttimecounter As Word 'Random Byte Dim Random As Byte '################## End of the shit part ################### Declare Sub Initudo() Declare Sub Settime() Declare Sub Gettime() Declare Sub Makenewtimestamp() Declare Sub Parammenu() Declare Sub Manualdrive() Declare Sub Timesetmenu() Declare Sub Mowsetmenu() Declare Sub Calibratemenu() Declare Sub Updatedisplay() Declare Sub Displaytime() Declare Sub Displaytempandhumi() Declare Sub Displaymowrpmcounter() Declare Sub Displayruntime() Declare Sub Displaydistance() Declare Sub Stopmotorsandpwm() Declare Sub Stopdrivemotor(byval Stopmode As Byte) Declare Sub Startdrivemotor(byval Startmode As Byte) Declare Sub StartMowMotor() Declare Sub EmergyStop() Declare Sub Flushremote() Declare Sub Rfleftdetected() Declare Sub Rfmiddetected() Declare Sub Rfrightdetected() Declare Sub Bumperdetected() Declare Sub Debugscreen() Declare Sub Setnewdrivedirection(byval Setdirection As Byte , Byval Setdrivemode As Byte) Declare Sub TurnUdo(Byval TurnDir As Byte,Byval WheelSteps As Byte) Declare Function Systimesec() As Long Declare Function Getremote() As Byte Declare Function Getremotebyte(byval Rc5ypos As Byte , Byval Rc5xpos As Byte , Byval Rc5maxval As Byte , Byvalrc5lenght As Byte) As Byte Declare Function Getad(byval Chselect As Byte) As Byte Declare Function RandomDir() As Byte Declare Function NewValue(ByVal PosY As Byte,ByVal PosX As Byte,ByVal InValue As Byte,ByVal Steps As Byte, ByVal MaxValue As Byte) As Byte '################# Main ########################## Call Initudo() #If MowmotorOn Call StartMowMotor() #EndIf Do If Leftrfdetect = True Then Call Rfleftdetected() If Midrfdetect = True Then Call Rfmiddetected() If Rightrfdetect = True Then Call Rfrightdetected() If Bumperdetect = True Then Call Bumperdetected() Systemruntimesec = Systimesec() - Systemstarttimestampsec 'Update time elapsed after started Call Updatedisplay() If Systemruntimemin >= Mowtime Then : Call Stopmotorsandpwm() : Reset 12v_en : End If 'Stop Mowing 'If LostTimeCounter >= LostTime * 10 Then Call EmergyStop() End If 'Emergy stop Select Case Getremote() Case Menukey : Call Parammenu() 'If Ir Menu key then goto parameter menu Case Keykey : Call Manualdrive() 'If Ir Key key then goto manual drive Case Smartkey : Call Startdrivemotor(slow) 'If Ir Smartkey start driving Case Selectkey : Call Stopdrivemotor(slow) 'If Ir SelectKey stop driving Case 1 : If DebugMode = False Then DebugMode= True Else DebugMode= False 'If Ir 1 DebugScreen on/off Case 0 : Start Watchdog 'If Ir 0 Reset CPU End Select Toggle Port_a7 'Debug Pin 33 Loop End 'end program '################# End Main ###################### Sub Initudo() Rightpower = 0 Leftpower = 0 Mowpower = 0 Direction = Forwarddir Leftrfdetect = False Midrfdetect = False Rightrfdetect = False Bumperdetect = False DebugMode = False Cls Cursor Off Locate 2 , 9 : Lcd "UDO 5" Locate 4 , 1 : Lcd "Version 0.1d" Waitms 1000 Cls If Eramempty = Eramemptymask Then 'Fill Eram with defaults if empty E_mowtime = 10 E_losttime = 3 E_turntime = 2 E_revtime = 5 E_acctimeUp = 3 E_acctimeDown = 2 E_maxdrivepower = 100 E_maxmowpower = 10 E_leftrftrig = 20 E_midrftrig = 20 E_rightrftrig = 20 Eramempty = Eramfilledmask 'Set Eram filled mask End If Mowtime = E_mowtime 'Copy Eram parameters to memory to save eram life Losttime = E_losttime Turntime = E_turntime Revtime = E_revtime AcctimeUp = E_acctimeUp AcctimeDown = E_acctimeDown Maxdrivepower = E_maxdrivepower Maxmowpower = E_maxmowpower Leftrftrig = E_leftrftrig Midrftrig = E_midrftrig Rightrftrig = E_rightrftrig 'Make Timestamp of starttime Call Makenewtimestamp() 'Enable the used interrupts Enable Interrupts #If PwmOn Enable Timer1 Enable Timer2 #EndIf Enable Int0 Enable Int1 Enable Int2 'Enable Hall sensors call On Int0 Hallsensormowmotor On Int1 Hallsensorleftmotor On Int2 Hallsensorrightmotor 'Enable PWM call On Timer2 Timertick30khz 'Enable Sensor Check On Timer1 TimerTick2Hz #If RelaisOn 'Enable 12v for motors Set 12v_en #EndIf End Sub Sub Manualdrive() Local AutoSpeedUp As Byte Disablesensors = True AutoSpeedUp = False 'Disable sensors Cls : Locate 1 , 1 : Lcd "Manual Drive" Call Stopdrivemotor(slow) Locate 4 , 1 : Lcd "Stop " Call Flushremote() Waitms 200 Do Select Case Getremote() Case Upkey Locate 4 , 1 : Lcd "Forward" AutoSpeedUp= True Call Stopdrivemotor(fast) Direction = Forwarddir Call Startdrivemotor(slow) Case Downkey Locate 4 , 1 : Lcd "Reverse" AutoSpeedUp= True Call Stopdrivemotor(fast) Direction = Reversedir Call Startdrivemotor(slow) Case Rightkey Locate 4 , 1 : Lcd "Right " If LeftPower >= 10 Then LeftPower= LeftPower - 10 Case Leftkey Locate 4 , 1 : Lcd "Left " If RightPower >= 10 Then RightPower= RightPower - 10 Case Okkey Locate 4 , 1 : Lcd "Stop " AutoSpeedUp= False Call Stopdrivemotor(fast) Case NoRC5 If AutoSpeedUp = True Then Call Startdrivemotor(Fast) End Select Loop Until Getremote() = Keykey Cls Call Stopdrivemotor(slow) Disablesensors = False 'Enable sensors End Sub Sub Rfleftdetected() 'Drive back and turn Disablesensors = True 'Disable sensors Leftrfdetect = False Call Setnewdrivedirection(rightdir , Slowstop) Losttimecounter = 0 'ResetLostTimeConter Disablesensors = False 'Enable sensors End Sub Sub Rfmiddetected() Local RDir As Byte 'Drive back and turn Disablesensors = True 'Disable sensors\ Midrfdetect = False Rdir = RandomDir() Call Setnewdrivedirection(RDir , Slowstop) Losttimecounter = 0 'ResetLostTimeConter Disablesensors = False 'Enable sensors End Sub Sub Rfrightdetected() 'Drive back and turn Disablesensors = True 'Disable sensors Rightrfdetect = False Call Setnewdrivedirection(leftdir , Slowstop) Losttimecounter = 0 'ResetLostTimeConter Disablesensors = False 'Enable sensors End Sub Sub Bumperdetected() Local RDir As Byte 'Drive back and turn Disablesensors = True 'Disable sensors Bumperdetect = False Rdir = RandomDir() Call Setnewdrivedirection(RDir , Faststop) Losttimecounter = 0 'ResetLostTimeConter Disablesensors = False 'Enable sensors End Sub Sub Setnewdrivedirection(setdirection As Byte , Setdrivemode As Byte) Call Stopdrivemotor(setdrivemode) Direction = Reversedir Call Startdrivemotor(slow) 'Drive reverse WaitMs Revtime 'Time for reverse driving Call Stopdrivemotor(slow) Direction = Setdirection Call Startdrivemotor(slow) 'Make Turn WaitMs Turntime 'Time for turning Call Stopdrivemotor(slow) Direction = Forwarddir Call Startdrivemotor(slow) 'Drive forward Random= 0 End Sub Sub TurnUdo(TurnDir As Byte, WheelSteps As Byte) 'Turns udo 45 steps is one wheelturn = +/- 75cm Local LeftRpmCalc As Long Local RightRpmCalc As Long LeftRpmCalc= LeftRpm + WheelSteps 'Setpoint to start with RightRpmCalc= RightRpm + WheelSteps Direction= TurnDir 'Now set the turn direction Do If LeftPower < 100 Then Incr LeftPower If RightPower < 100 Then Incr RightPower If LeftRpmCalc <= LeftRpm Then LeftPower= 0 'This sucks in absolute turn measurement, but is suitable enough for UDO If RightRpmCalc <= RightRpm Then RightPower= 0 WaitMs 40 Loop Until LeftPower=0 And LeftPower=0 End Sub Function RandomDir() As Byte RandomDir= Random Mod 2 Incr RandomDir End Function Sub Gettime() If I2cbusbusy = True Then Exit Sub 'Don't get the time if i2c bus is busy for GetAD I2cbusbusy = True 'The high level call Rtcarray(1) = 2 ' point to second register I2creceive &HA0 , Rtcarray(1) , 1 , 6 ' write the second address and get 6 bytes back I2cbusbusy = False End Sub Sub Settime() 'values are stored as BCD values so convert the values first Rtcarray(1) = Makebcd(rtcseconde) 'seconds Rtcarray(2) = Makebcd(rtcminute) 'minutes Rtcarray(3) = Makebcd(rtchour) 'hours Rtcarray(4) = Makebcd(rtcday) 'days Rtcarray(5) = Makebcd(rtcmonth) 'months Rtcarray(6) = Makebcd(rtcyear) 'Year I2cstart 'generate start I2cwbyte &HA0 'write address I2cwbyte 0 'select control register I2cwbyte 8 'set year and day bit for masking I2cstart 'repeated start I2cwbyte &HA0 'write mode I2cwbyte 2 'select seconds Register For Rtccount = 1 To 6 I2cwbyte Rtcarray(rtccount) Next 'write seconds I2cstop End Sub Sub Systimesec() Local Systimetmp1 As Long Local Systimetmp2 As Long Local Systimetmp3 As Long Systimetmp3 = Makedec(day) 'Since RTC is in BCD make it DEC Systimetmp2 = Systimetmp3 * 86400 Systimetmp1 = Systimetmp2 Systimetmp3 = Makedec(hour) Systimetmp2 = Systimetmp3 * 3600 Systimetmp1 = Systimetmp1 + Systimetmp2 Systimetmp3 = Makedec(minute) Systimetmp2 = Systimetmp3 * 60 Systimetmp1 = Systimetmp1 + Systimetmp2 Systimetmp3 = Makedec(seconde) Systimetmp2 = Systimetmp3 Systimetmp1 = Systimetmp1 + Systimetmp2 Systimesec = Systimetmp1 End Sub Sub Makenewtimestamp() Call Gettime() Systemstarttimestampsec = Systimesec() End Sub Function Getad(chselect As Byte) As Byte Local Pcf8591out As Byte If I2cbusbusy = True Then Exit Sub 'Don't get the AD if i2c bus is busy for gettime I2cbusbusy = True I2cstart I2cwbyte Pcf8591write 'Set PCF8591 to Write. I2cwbyte Chselect 'Control byte to select channel 0..3 I2cstart I2cwbyte Pcf8591read 'Set PCF8591 to Read. I2crbyte Pcf8591out , Ack 'Read PCF8591 I2crbyte Pcf8591out , Nack I2cstop I2cbusbusy = False Getad = Pcf8591out End Function Sub Updatedisplay() 'Disable Timer2 'No Intr during display update Select Case DebugMode Case False : Call Displaytime() Call Displaytempandhumi() Call Displaymowrpmcounter() Call Displaydistance() Call Displayruntime() Case True : Call DebugScreen() End Select 'Enable Timer2 End Sub Sub Displaytime() Call Gettime() 'since the values are stored in BCD format we can use Hex() to display them Locate 4 , 13 : Lcd Spc(8) : Locate 4 , 13 Lcd Hex(hour) ; ":" ; Hex(minute) ; ":" ; Hex(seconde) 'LCD Hex(Day);"/";Hex(Month);"/";Hex(Year);" ";Hex(Hour);":";Hex(Minute);":";Hex(Seconde) End Sub Sub Displaytempandhumi() 'Call GetSHT11 Locate 1 , 18 Lcd "20" ; Chr(1) 'Chr(1) is Celcius symbol Locate 2 , 18 Lcd "60%" End Sub Sub Displaymowrpmcounter() Local Timeelapsedmowrpm As Long Local Mowrpm As Long Timeelapsedmowrpm = Systimesec() - Prevtimemowcounter If Timeelapsedmowrpm < 10 Then Exit Sub 'Only update every 10 sec for more acure Prevtimemowcounter = Systimesec() Mowrpm = Mowrpmcounter \ Timeelapsedmowrpm Locate 1 , 1 : Lcd Spc(16) : Locate 1 , 1 Mowrpm = Mowrpm * 60 'Multiply by 60 for minutes Lcd Mowrpm ; " RPM" Mowrpmcounter = 0 End Sub Sub Displaydistance() Local Distance As Long Locate 2 , 1 : Lcd Spc(17) : Locate 2 , 1 Distance = 78 * LeftRpm Distance = Distance / 100 Lcd Distance ; " M" End Sub Sub Displayruntime() Systemruntimemin= Systemruntimesec \ 60 Locate 4 , 1 : Lcd Spc(12) : Locate 4 , 1 Lcd Systemruntimemin ; "/" ; Mowtime ; " Min" End Sub Sub Debugscreen() Locate 1,1 : Lcd "L-RF M-RF R-RF Bump " Locate 3,1 : Lcd "L-Pow R-Pow M-Pow" Locate 2,1 : Lcd Spc(20) Locate 2,1 : Lcd "[" ; Getad(2) ; "]" Locate 2,6 : Lcd "[" ; Getad(1) ; "]" Locate 2,11: Lcd "[" ; Getad(0) ; "]" Locate 2,16 If BumperDetect = True Then Lcd "[On]" Else Lcd "[Off]" Locate 4,1 : Lcd Spc(20) Locate 4,1 : Lcd "[" ; Leftpower ; "]" Locate 4,7 : Lcd "[" ; Rightpower; "]" Locate 4,13: Lcd "[" ; Mowpower ; "]" End Sub Sub Parammenu() Cls Call Flushremote() Call Stopmotorsandpwm() 'Stop motors and PWM interrupts to speedup Do Locate 1 , 1 : Lcd "Parameter menu" Locate 2 , 1 : Lcd "1) Time settings" Locate 3 , 1 : Lcd "2) Mow settings" Locate 4 , 1 : Lcd "3) RF calibration" Select Case Mainmenuchoice Case 1 : Call Timesetmenu() Case 2 : Call Mowsetmenu() Case 3 : Call Calibratemenu() End Select Mainmenuchoice = Getremote() Loop Until Mainmenuchoice = Menukey Enable Timer2 'Enable PWM background task Call StartMowMotor() Cls End Sub Sub Timesetmenu() Cls Cursor On Call Flushremote() Locate 1 , 1 : Lcd "Day : " Rtcday = Getremotebyte(1 , 7 , 31 , 2) Locate 2 , 1 : Lcd "Mon : " Rtcmonth = Getremotebyte(2 , 7 , 12 , 2) Locate 3 , 1 : Lcd "Year: " Rtcyear = Getremotebyte(3 , 7 , 10 , 2) Cls Locate 1 , 1 : Lcd "Hour: " Rtchour = Getremotebyte(1 , 7 , 24 , 2) Locate 2 , 1 : Lcd "Min : " Rtcminute = Getremotebyte(2 , 7 , 60 , 2) Locate 3 , 1 : Lcd "Sec : " Rtcseconde = Getremotebyte(3 , 7 , 60 , 2) Call Settime() 'set time Cls Cursor Off End Sub Sub Mowsetmenu() Cls Cursor Off Call Flushremote() Locate 1,1 : Lcd "Mowtime :" Eparsebyte = E_mowtime EparseByte = NewValue(1,11,EparseByte,10,180) E_mowtime = Eparsebyte Locate 2,1 : Lcd "LostTime:" Eparsebyte = E_losttime EparseByte = NewValue(2,11,EparseByte,1,10) E_losttime = Eparsebyte Locate 3,1 : Lcd "TurnTime:" Eparsebyte = E_turntime EparseByte = NewValue(3,11,EparseByte,1,10) E_turntime = Eparsebyte Locate 4,1 : Lcd "RevTime :" Eparsebyte = E_revtime EparseByte = NewValue(4,11,EparseByte,1,10) E_revtime = Eparsebyte Cls Locate 1,1 : Lcd "AccTimeUp :" Eparsebyte = E_acctimeUp EparseByte = NewValue(1,14,EparseByte,1,10) E_acctimeUp = Eparsebyte Locate 2,1 : Lcd "AccTimeDown:" Eparsebyte = E_acctimeDown EparseByte = NewValue(2,14,EparseByte,1,10) E_acctimeDown = Eparsebyte Locate 3,1 : Lcd "MaxDrivePow:" Eparsebyte = E_maxdrivepower EparseByte = NewValue(3,14,EparseByte,1,100) E_maxdrivepower = Eparsebyte Locate 4,1 : Lcd "MaxMowPow :" Eparsebyte = E_maxMowPower EparseByte = NewValue(4,14,EparseByte,1,100) E_maxMowpower = Eparsebyte Start Watchdog 'Full Reset End Sub Sub Calibratemenu() Cls Cursor Off Call Flushremote() Lcd "1=Left 2=Mid 3=Right" Locate 4 , 1 : Lcd "[1..3] to calibrate" Do Locate 2,1 : Lcd "[" ; Getad(2) ; "]" Locate 2,8 : Lcd "[" ; Getad(1) ; "]" Locate 2,14 : Lcd "[" ; Getad(0) ; "]" Select Case Getremote() Case 1 Eparsebyte = Getad(2) E_leftrftrig = Eparsebyte Leftrftrig = E_leftrftrig Locate 3 , 3 : Lcd "*" Case 2 Eparsebyte = Getad(1) E_midrftrig = Eparsebyte Midrftrig = E_midrftrig Locate 3 , 10 : Lcd "*" Case 3 Eparsebyte = Getad(0) E_rightrftrig = Eparsebyte Rightrftrig = E_rightrftrig Locate 3 , 16 : Lcd "*" End Select Loop Until Getremote() = Okkey Cls Locate 4 , 1 : Lcd Spc(20) : Locate 4 , 1 : Lcd "Saved" Waitms 1000 End Sub Sub StopMotorsAndPwm() Call Stopdrivemotor(slow) Mowpower = 0 Disable Timer2 End Sub Sub Stopdrivemotor(stopmode As Byte) If Stopmode = Slow Then Do If Rightpower > 0 Then Decr Rightpower If Leftpower > 0 Then Decr Leftpower Waitms AcctimeDown Loop Until Rightpower = 0 Else Rightpower = 0 Leftpower = 0 End If End Sub Sub Startdrivemotor(startmode As Byte) If Startmode = Slow Then Do If Rightpower < Maxdrivepower Then Incr Rightpower 'stapjes van 10?? If Leftpower < Maxdrivepower Then Incr Leftpower Waitms AcctimeUp Loop Until Rightpower = Maxdrivepower 'dit rammelt !!! moet beter Else Rightpower = Maxdrivepower Leftpower = Maxdrivepower End If End Sub Sub StartMowMotor() Do Incr MowPower WaitMs 5 Loop Until MowPower = MaxMowPower End Sub Sub EmergyStop() Reset 12v_en Disable Timer2 Rightpower = 0 Leftpower = 0 Mowpower = 0 End Sub Function Getremotebyte(rc5ypos As Byte , Rc5xpos As Byte , Rc5maxval As Byte , Rc5lenght As Byte) As Byte Local Rc5buffer As Word Local Rc5count As Byte Local Rc5inkey As Byte Local Rc5xd As Byte Do Rc5buffer = 0 Rc5count = Rc5lenght Rc5xd = 16 - Rc5lenght Locate 4 , 1 : Lcd Spc(20) Locate 4 , 1 : Lcd "Len [" ; Rc5lenght ; "]" Locate 4 , Rc5xd : Lcd "[0.." ; Rc5maxval ; "]" Locate Rc5ypos , Rc5xpos Lcd Spc(rc5lenght) Locate Rc5ypos , Rc5xpos Do Rc5inkey = Getremote() If Rc5inkey <> Norc5 Then If Rc5inkey <= 9 Then Lcd Rc5inkey If Rc5count <> Rc5lenght Then Rc5buffer = Rc5buffer * 10 Rc5buffer = Rc5buffer + Rc5inkey Decr Rc5count End If End If Waitms 100 Loop Until Rc5count = 0 Loop Until Rc5buffer <= Rc5maxval Getremotebyte = Rc5buffer End Function Function NewValue(PosY As Byte, PosX As Byte, InValue As Byte, Steps As Byte, MaxValue As Byte) Local NewValueInp As Byte Locate PosY,PosX : LCD InValue Do NewValueInp = GetRemote() Locate PosY,PosX : LCD InValue Select Case NewValueInp Case DownKey Locate PosY,PosX : LCD Spc(3) If InValue > 0 Then InValue = InValue - Steps Case UpKey Locate PosY,PosX : LCD Spc(3) If InValue < MaxValue Then InValue= InValue + Steps End Select WaitMs 100 Loop Until NewValueInp = OkKey NewValue= InValue End Function Function Getremote() As Byte Local AddrRC5 As Byte Local CommandRc5 As Byte Getrc5(AddrRC5 , CommandRc5) If AddrRC5 = 0 Then 'TV remote Command CommandRc5 = CommandRc5 And &B01111111 'Remove Toggel bit of RC6 Else Getremote = Norc5 'My nothing received signal (255) End If If CommandRc5 = SleepKey Then Call EmergyStop() 'SleepKey for emergency stop Waitms 100 End If Getremote = CommandRc5 End Function Sub Flushremote() Local Flushtmp As Byte While Flushtmp <> Norc5 Flushtmp = Getremote() Wend End Sub TimerTick2Hz: 'Set direction part Toggle Port_d7 'Debug pin 21 2Hz/2 by toggle Tmpportc = Portc And &B00111111 'remove first 2 bits If Tmpportc <> Direction Then 'Direction was changed set new direction Portc = Portc And &B11111100 'Clear direction Portc = Portc + Direction 'Set direction End If If Disablesensors = False Then #If SensorsOn If Getad(2) > Leftrftrig Then Leftrfdetect = True If Getad(1) > Midrftrig Then Midrfdetect = True If Getad(0) > Rightrftrig Then Rightrfdetect = True #EndIf #If BumperOn If Bumper_in = LowLevel Then Bumperdetect = True #EndIf Incr Losttimecounter EndIf Return Timertick30khz: If Pwmclk >= Leftpower Then Set Pwm_left Else Reset Pwm_left If Pwmclk >= Rightpower Then Set Pwm_right Else Reset Pwm_right If Pwmclk >= Mowpower Then Reset Mowmotor_out Else Set Mowmotor_out If Pwmclk < 99 Then Incr Pwmclk Else Pwmclk = 0 Incr Random Return Hallsensormowmotor: Incr Mowrpmcounter Return Hallsensorleftmotor: Incr Leftrpm Return Hallsensorrightmotor: Incr Rightrpm Return