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
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:)