Post by nixa on Jul 19, 2021 13:36:35 GMT -5
I am working on robot mower with SLAM (Simultaneous localization and mapping) capability and I would like to share some code and hear what you think about it. Project is going slow due to family, work and what not...
As in my question at this thread, navigation software will be split into multiple instances. We'll see how will that go...
In linked thread, I have stated that I'll be using laser, but they are expensive AF. So I have settled to use ultrasonic sensors. Same principle, but shorter range.
Anyway, I have salvaged an old Asus eee PC with 900MHz cpu, swaped HDD with SSD, installed Windows XP and got it to read GPS data from USB receiver. Here is some testing code:
Hopefuly, everyone understands that earth is round, and maps we all use, are flat. So to make it simpler for me and computer, above code, converts polar coordinates to planar. It allso converts units from kilometers to decimeters witch will be the resolution of my map.
Next step would be to mount laptop on a robot and actualy create extremities map. Just border points to get operation limits so it doesent mowe my neighbours corn.
EDIT: Fixed some errors in code.
As in my question at this thread, navigation software will be split into multiple instances. We'll see how will that go...
In linked thread, I have stated that I'll be using laser, but they are expensive AF. So I have settled to use ultrasonic sensors. Same principle, but shorter range.
Anyway, I have salvaged an old Asus eee PC with 900MHz cpu, swaped HDD with SSD, installed Windows XP and got it to read GPS data from USB receiver. Here is some testing code:
WindowWidth = 280
WindowHeight = 202
UpperLeftX = INT((DisplayWidth-WindowWidth)/2)
UpperLeftY = INT((DisplayHeight-WindowHeight)/2)
statictext #main.1, "X", 35, 15, 15, 20
statictext #main.2, "Y", 35, 50, 15, 20
statictext #main.3, "Mode:", 10, 85, 40, 20
statictext #main.4, "Valid", 15, 125, 35, 16
statictext #main.5, "dm", 160, 15, 20, 20
statictext #main.6, "dm", 160, 50, 20, 20
textbox #main.textbox1, 50, 10, 100, 30
textbox #main.textbox2, 50, 45, 100, 30
textbox #main.textbox3, 50, 80, 100, 30
textbox #main.textbox4, 50, 115, 100, 30
Open "Window Title" for Window_nf as #main
#main "trapclose [quit]"
goto [skipcom] 'skip com port for debuging
'open "COM3:9600,n,8,1" for input as #com
'timer 500, [readgps] 'read info from gps device every ### ms
[loop]
Wait
[quit]
close #main
'close #com
end
[readgps]
if lof(#com) > 750 then
t$ = input$(#com, lof(#com)) 'read gps data to tempolary variable
while left$(t$, 6) <> "$GNGLL" and len(t$) > 6 ' search for beginning of relevant line
t$ = right$(t$, len(t$) - 1) 'remove first character untill relevant data is found
wend
lat$ = word$(t$, 2, ",") 'extract latitude data - format ddmm.mmmmm
lon$ = word$(t$, 4, ",") 'extract longitude data - - format ddmm.mmmmm
valid$ = word$(t$, 7, ",") 'not sure what this does
mode$ = left$(word$(t$, 8, ","), 1) 'operation mode
[skipcom] ' for debuging, skip to this line
lon$ = "1234.567890" 'debug - insert your coordinates
lat$ = "0987.654321" 'debug - insert your coordinates
latPdeg = 111.133 'km@45deg
pi180 = 0.0174532925199432 'pi devided by 180
earthR = 6367.205 'km@46,3 lat - 200m alt - https://rechneronline.de/earth-radius/
la = val(left$(lat$,2)) + val(right$(lat$, len(lat$)-2))/60 'convert latitude to decimal
lo = val(left$(lon$,3)) + val(right$(lon$, len(lon$)-3))/60 'convert longitude to decimal
latrad = la * (pi180) 'latitude in radians
x = ((pi180*earthR*cos(latrad))*lo)*10000 'in decimeters
y = (latPdeg * la)*10000 'in decimeters
print #main.textbox1, using("########.##",x)
print #main.textbox2, using("########.##",y)
select case mode$
case "N"
mode$ = "No fix"
case "E"
mode$ = "Dead reckoning" 'https://www.furuno.com/en/gnss/technical/tec_dead
case "D"
mode$ = "Differential" 'https://en.wikipedia.org/wiki/Differential_GPS
case "A"
mode$ = "Autonomous"
case else
end select
select case valid$
case "A"
valid$ = "Valid"
case "V"
valid$ = "Invalid"
end select
print #main.textbox3, mode$
print #main.textbox4, valid$
print #main, "refresh"
end if
wait
Hopefuly, everyone understands that earth is round, and maps we all use, are flat. So to make it simpler for me and computer, above code, converts polar coordinates to planar. It allso converts units from kilometers to decimeters witch will be the resolution of my map.
Next step would be to mount laptop on a robot and actualy create extremities map. Just border points to get operation limits so it doesent mowe my neighbours corn.
EDIT: Fixed some errors in code.