Mapper robot

C, C++, Pascal, Assembly, Raspberry, Java, Matlab, Python, BASIC, SQL, PHP, etc.
jntron
Inlägg: 40
Blev medlem: 7 juni 2018, 20:01:00

Mapper robot

Inlägg av jntron »

Hej!
Jobbar på ett robotprojekt som kör runt slumpartat och undviker hinder, för att sedan skapa en "karta" över områden den varit i.
Har äntligen fått roboten att köra runt som jag vill, men det är något allvarligt fel med kartdelen.
Bifogar python kod som skapar en .avi

Kod: Markera allt

import cv2
import numpy as np
import math

fourcc = cv2.VideoWriter_fourcc(*'XVID')
video = cv2.VideoWriter("map.avi", fourcc, 25, (1024, 1024), True)

with open("datat.txt", "r") as f:
    lines = f.readlines()

speed = None
heading = None
servo_heading = None

[t, s, v] = lines.pop(0).strip().split(", ")
previous_time = float(t)

motor_status = "stop"

got_speed = False
distance1 = None
time1 = None

ratio = 0.5


class Pixel(object):

    def __init__(self, x, y, color):
        self.x = np.clip(x*ratio, -512, 511) + 512
        self.y = np.clip(y*ratio, -512, 511) + 512
        self.color = color


pixels = []

robot_x = robot_y = 0.0

fps = 25
frame_time = 0

for line in lines:
    [timestamp, service, value] = line.strip().split(", ")
    timestamp = float(timestamp)
    #print(line)

    dt = (timestamp - previous_time) / 1000.0 # i sekunder
    previous_time = timestamp

    if got_speed:
        frame_time += dt

        if service == "motors":
            motor_status = value
        elif service == "compass":
            heading = float(value)
        elif service == "sonar":
            distance = float(value)

            x = robot_x + math.sin(math.radians(heading + servo_heading)) * distance
            y = robot_y + math.cos(math.radians(heading + servo_heading)) * distance

            pixels.append(Pixel(x, y, (255, 0, 0)))
        elif service == "servo":
            servo_heading = (float(value)-50)/100*90

        if motor_status == "forward" and heading is not None:
            robot_x += math.sin(math.radians(heading)) * speed * dt
            robot_y += math.cos(math.radians(heading)) * speed * dt
            pixels.append(Pixel(robot_x, robot_y, (0, 255, 0)))

    if frame_time >= 1.0/fps:
        frame_time = 0
        frame = np.zeros((1024, 1024, 3), np.uint8)
        for pix in pixels:
            cv2.circle(frame, (int(pix.x), int(pix.y)),  2, pix.color, -1)
            #frame[int(pix.x), int(pix.y)] = pix.color

        video.write(frame)

    if not got_speed:
        if service == "speed1":
            distance1 = float(value)
            time1 = float(timestamp)/1000
        elif service == "speed2":
            travelled = float(value) - distance1
            time = float(timestamp)/1000 - time1
            speed = math.fabs(travelled/time)  # cm/s
            motor_status = "forward"
            got_speed = True
Någon som ser nått uppenbart fel här?
datat.txt (https://pastebin.com/5CMZfCre)
är output från roboten, tre kolumner (timestamp i millisekunder, service, value)
servo värdet är räknat i procent där 50 är rakt framåt, alla vinklar är i grader
Jag misstänker även att det är något fel med kompassmodulen.. Men det förklarar nog inte allt så det är nog fel här också
Har kört fast med python koden, så alla tips är guld värda:)
johano
Inlägg: 1943
Blev medlem: 22 januari 2008, 10:07:45
Ort: Stockholm

Re: Mapper robot

Inlägg av johano »

En liten beskrivning av själva problemet skulle nog öka dina chanser till ett bra svar.

/johan
jntron
Inlägg: 40
Blev medlem: 7 juni 2018, 20:01:00

Re: Mapper robot

Inlägg av jntron »

Hej,
ska försöka, lite svårt när jag inte är 100% säker var problemet ligger...
Jag har ett sonar på ett servo i främre delen av roboten som åker mellan vänster-höger i ett intervall
som verkar ge OK resultat när det gäller att undvika väggar, men när jag försöker generera en karta baserad på data från dessa inläsningar ihopsamlad i en textfil så får jag nonsens..
Så jag tror att problemet ligger i koden jag klistrade in, men jag är inte helt säker
Har slitit med det här projektet en längre tid nu och det känns som målet är någonstans i sikte, men har fastnat på detta.
Sonaret är ett HC-SR04, någon annan som har testat och fått bra resultat med detta?
Kan det vara så illa att man måste stå still när man gör en sonar läsning?
En sak jag funderar på, kan det vara gamla ljudvågor från sonaret som fortfarande studsar runt och stör mätningarna?
Tacksam för alla svar.
Användarvisningsbild
Micke_s
EF Sponsor
Inlägg: 6741
Blev medlem: 15 december 2005, 21:31:34
Ort: Malmö

Re: Mapper robot

Inlägg av Micke_s »

Inte så du slirar på något hjul och den tror att den har gått längre än i verkligheten.
jntron
Inlägg: 40
Blev medlem: 7 juni 2018, 20:01:00

Re: Mapper robot

Inlägg av jntron »

Kör ju larvfötter https://hobbyking.com/en_us/rover-5-tra ... assis.html
så det borde väl inte slira? Jag räknar ut hastigheten genom att time:a 2 sonar läsningar mot en vägg med motorerna igång och använder det i uträkningarna, vilket kanske är dumt eftersom roboten inte går från 0 till ca 25cm/s direkt, och samma sak när motorerna stoppas, den kanske kör vidare en bit innan den stannar helt.
Chassit har rotary encoders, använder inte de just nu men det kanske skulle ge mer exakta positioner..
Tror inte att det här heller kan fullt förklara mina slumpade läsningar, men det kan vara en hel del problem som spökar samtidigt och inte bara ett.
Användarvisningsbild
hawkan
Inlägg: 2586
Blev medlem: 14 augusti 2011, 10:27:40

Re: Mapper robot

Inlägg av hawkan »

Datan såg väl inte så slumpmässig ut?
Jag tror att i ett sånt här läge är det bara att kavla upp ärmar
och plocka fram stora tålamodet. Och sedan gå igenom koden rad för rad
med den input du har i datafilen. Gå igenom varje rad och dubbelkolla att
det blir det som du har tänkt dej. Insikten ökar och till slut kan det bli
ett "aha oj". Eller nåt.
Alternativt om den inspelade datafilen kan födas in i programmet och
du lägger in massor av debugutskrifter. Och verkligen kollar av de rad för rad.

Jag kan nätt och jämt läsa python, men detta sätt brukar funka när det inte blir som man vill.
jntron
Inlägg: 40
Blev medlem: 7 juni 2018, 20:01:00

Re: Mapper robot

Inlägg av jntron »

Du har nog rätt, får försöka köra koden steg för steg och jämföra vad som händer... Hoppades att det skulle visa sig vara ett enkelt mattefel i kartkoden men jag har ingen riktig koll, så det blir att fördjupa sig och försöka debugga så gott jag kan, det är ett multitrådat åbäke så jag försökte undvika det :)
Tack!
Användarvisningsbild
Krille Krokodil
Inlägg: 4062
Blev medlem: 9 december 2005, 22:33:11
Ort: Helsingborg

Re: Mapper robot

Inlägg av Krille Krokodil »

Den ultraljudssensorn ger en pulsbredd tillbaka som ska mätas med us-timing, hur görs det?
jntron
Inlägg: 40
Blev medlem: 7 juni 2018, 20:01:00

Re: Mapper robot

Inlägg av jntron »

c++ kod med biblioteket wiringPi (obs tror jag hittade detta någonstans på internet):

Kod: Markera allt

float Sonar::read() {
    digitalWrite(10, HIGH);
    delayMicroseconds(20);
    digitalWrite(10, LOW);
    long start, stop;
    long now = micros();
    while(digitalRead(11) == LOW && micros()-now < 1000000);
    start = micros();
    while(digitalRead(11) == HIGH && micros()-start < 1000000);
    stop = micros();

    float time = (stop-start) / 58.0f;
    return time;
}
kodar-holger
EF Sponsor
Inlägg: 916
Blev medlem: 26 maj 2014, 12:54:35
Ort: Karlskoga

Re: Mapper robot

Inlägg av kodar-holger »

Hmmm. Undrar om du fått sin och cos förväxlade kanske.

I vilken riktning anser du x och y axlarna vara på roboten respektive i verkligheten?

Sen tror jag att dina antaganden om att larvfötter skulle vara bättre än hjul kan spela dig en del spratt. I synnerhet när du svänger.

Funkar det när du står stilla?
jntron
Inlägg: 40
Blev medlem: 7 juni 2018, 20:01:00

Re: Mapper robot

Inlägg av jntron »

Jo stillastående verkar det inte vara något problem.
Tänkte att kompassgrad 0 är positivt Y...
kodar-holger
EF Sponsor
Inlägg: 916
Blev medlem: 26 maj 2014, 12:54:35
Ort: Karlskoga

Re: Mapper robot

Inlägg av kodar-holger »

Jag är skeptisk till din metod att få fram robotens position. Om kartan blir rimlig stillastående så är det väl två saker man tänker på direkt.

Sensorn klarar inte att du rör dig. Det känns osannolikt. Visst lite dopplerskift blir det, men knappast så mycket att det borde få sensorn att misslyckas. 5 promille vid 1m/s och så fort tror jag inte du kör..

Så är det positionsberäkningen. Kan du plotta bara den medan du kör runt på något sätt? Jag verkar inte kunna köra din kod för det fattas några paket i min pythoninstallation.

Kör du på en RaspberryPi? Vad har du för OS? Vanlig Linux är inte direkt något realtids-os så det kan bli rätt mycket jitter i dina avläsningar om det är den som mäter reflektionstiden.
jntron
Inlägg: 40
Blev medlem: 7 juni 2018, 20:01:00

Re: Mapper robot

Inlägg av jntron »

Hej,
Jag är också inne på att det är något galet med position uträkningen
Håller på att skriva om koden just nu för att försöka förenkla debugging..
Tänkte kolla om jag får en ok karta om jag kör en sekund, stannar och skannar, kör igen etc (egentligen inte hur jag vill ha det)
Jag tror även att kompassen är en stor del av problemet.

Du behöver numpy och opencv för att köra kart exemplet.

Jag kör en orange pi zero med armbian, har sett att det finns realtime linux, skulle det vara värt att testa det?
kodar-holger
EF Sponsor
Inlägg: 916
Blev medlem: 26 maj 2014, 12:54:35
Ort: Karlskoga

Re: Mapper robot

Inlägg av kodar-holger »

Förmodligen är det mer besvär än det är värt att försöka sig på en RT-linux. Troligen mycket enklare att sätta en enkel billig pic eller atmel-krets mellan som får gå i realtid och mata din apelsinpaj med data i serieformat.

Kompassen kan naturligtvis påverkas av om motorerna körs. Magnetfält har svårt att hålla sig på sin plats. Men det är ju också en sak som vore rimligt enkel att testa för sig.
jntron
Inlägg: 40
Blev medlem: 7 juni 2018, 20:01:00

Re: Mapper robot

Inlägg av jntron »

Skippar RT-linux..
Tanken bakom att låta pi:n stå för all hårdvaru kommunikation var att försöka utnyttja allt jag kunde av processorn och bussarna.. Kanske var detta fel tänkt, man skulle ju till och med kunna ha mer än en atmega eller dylikt.. Men hårdvaran funkar (nästan) just nu och jag gör helst inga stora ändringar om jag kan få det att rulla utan att koppla om allt.

Angående kompassen, tror du det påverkar mätningarna om en motor kör framåt eller bakåt? Borde bli samma sak eller?
Skriv svar