Skip to content

Commit

Permalink
control
Browse files Browse the repository at this point in the history
  • Loading branch information
@organic-nailer
organic-nailer committed Nov 18, 2024
1 parent f571aa3 commit 451abf9
Show file tree
Hide file tree
Showing 2 changed files with 130 additions and 45 deletions.
39 changes: 39 additions & 0 deletions can_controller.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
import can
import time
import threading

class CanController:
def __init__(self) -> None:
self.bus = can.interface.Bus(bustype='socketcan', channel='can0', bitrate=500000)

self.message_to_send: dict[int, bytearray] = {}
self.message_received: dict[int, bytearray] = {}
self._sender_flag = True
self._receiver_flag = True

self.sender_thread = threading.Thread(target=self.run_sender)
self.sender_thread.start()
self.receiver_thread = threading.Thread(target=self.run_receiver)
self.receiver_thread.start()

def close(self):
self._sender_flag = False
self._receiver_flag = False
self.sender_thread.join()
self.receiver_thread.join()
self.bus.shutdown()

def run_sender(self):
while self._sender_flag:
for msg_id, msg_data in self.message_to_send.items():
msg = can.Message(arbitration_id=msg_id, data=msg_data, is_extended_id=False)
self.bus.send(msg)
time.sleep(0.01)

def run_receiver(self):
while self._receiver_flag:
for i, msg in enumerate(self.bus):
self.message_received[msg.arbitration_id] = msg.data
if i > 20:
break
time.sleep(0.05)
136 changes: 91 additions & 45 deletions hello.py → main.py
View file
Original file line number Diff line number Diff line change
@@ -1,44 +1,9 @@
import can
import time
import threading
import numpy as np
import cv2
import pygame

class CanController:
def __init__(self) -> None:
self.bus = can.interface.Bus(bustype='socketcan', channel='can0', bitrate=500000)

self.message_to_send: dict[int, bytearray] = {}
self.message_received: dict[int, bytearray] = {}
self._sender_flag = True
self._receiver_flag = True

self.sender_thread = threading.Thread(target=self.run_sender)
self.sender_thread.start()
self.receiver_thread = threading.Thread(target=self.run_receiver)
self.receiver_thread.start()

def close(self):
self._sender_flag = False
self._receiver_flag = False
self.sender_thread.join()
self.receiver_thread.join()
self.bus.shutdown()

def run_sender(self):
while self._sender_flag:
for msg_id, msg_data in self.message_to_send.items():
msg = can.Message(arbitration_id=msg_id, data=msg_data)
self.bus.send(msg)
time.sleep(0.05)

def run_receiver(self):
while self._receiver_flag:
for i, msg in enumerate(self.bus):
self.message_received[msg.arbitration_id] = msg.data
if i > 20:
break
time.sleep(0.05)
from can_controller import CanController

def rotate_image(image, angle):
(h, w) = image.shape[:2]
Expand All @@ -47,7 +12,7 @@ def rotate_image(image, angle):
rotated = cv2.warpAffine(image, M, (w, h))
return rotated

def display_values(speed, throttle, brake, steer, steering_wheel_img):
def display_values(speed, throttle, brake, steer, steering_wheel_img, screen):
img = np.zeros((400, 600, 3), dtype=np.uint8)
cv2.putText(img, f"Speed: {speed:.2f} m/s", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
cv2.putText(img, f"Throttle: {throttle:.2f} %", (50, 100), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
Expand Down Expand Up @@ -78,11 +43,44 @@ def display_values(speed, throttle, brake, steer, steering_wheel_img):
for c in range(0, 3):
img[y1:y2, x1:x2, c] = (alpha_s * rotated_steering_wheel[:, :, c] +
alpha_l * img[y1:y2, x1:x2, c])

opencv_image = img[:,:,::-1]
shape = opencv_image.shape[1::-1]
pygame_image = pygame.image.frombuffer(opencv_image.tobytes(), shape, "BGR")
screen.blit(pygame_image, (0, 0))
pygame.display.flip()

cv2.imshow("Vehicle Data", img)
cv2.waitKey(1)
# cv2.imshow("Vehicle Data", img)
# cv2.waitKey(1)

def main():
pygame.joystick.init()
try:
joystick = pygame.joystick.Joystick(0)
joystick.init()
print("Joystick found:", joystick.get_name())
except pygame.error:
print("No joystick found")
return

pygame.init()
screen = pygame.display.set_mode((600, 400))

# while True:
# for event in pygame.event.get():
# if event.type == pygame.JOYAXISMOTION:
# print("Axis", event.axis, "motion", event.value)
# elif event.type == pygame.JOYBUTTONDOWN:
# print("Button", event.button, "down")
# elif event.type == pygame.JOYBUTTONUP:
# print("Button", event.button, "up")
# elif event.type == pygame.JOYHATMOTION:
# print("Hat", event.hat, "motion", event.value)
# elif event.type == pygame.QUIT:
# pygame.quit()
# return
# return

controller = CanController()
steering_wheel_img = cv2.imread("steering.jpg")
speed = 0.0
Expand All @@ -100,25 +98,73 @@ def main():
# speed_raw_signed = -((speed_raw ^ 0xFFFF) + 1) if report_vcu[2] & 0x80 else speed_raw
# speed = speed_raw_signed / 1000.0 # m/s
speed = int.from_bytes(report_vcu[2:4], 'big', signed=True) / 1000.0
print(f"Speed: {speed} m/s")
# print(f"Speed: {speed} m/s")
if 0x500 in received_data:
report_throttle = received_data[0x500]
# throttle = ((report_throttle[3] << 8) + report_throttle[4]) / 10.0 # %
throttle = int.from_bytes(report_throttle[3:5], 'big') / 10.0
print(f"Throttle: {throttle} %")
# print(f"Throttle: {throttle} %")
if 0x501 in received_data:
report_brake = received_data[0x501]
# brake = ((report_brake[3] << 8) + report_brake[4]) / 10.0
brake = int.from_bytes(report_brake[3:5], 'big') / 10.0
print(f"Brake: {brake} %")
# print(f"Brake: {brake} %")
if 0x502 in received_data:
report_steer = received_data[0x502]
# steer = ((report_steer[3] << 8) + report_steer[4]) - 500
steer = int.from_bytes(report_steer[3:5], 'big') - 500
print(f"Steering: {steer} deg")
# print(f"Steering: {steer} deg")

# Display vehicle data
display_values(speed, throttle, brake, steer, steering_wheel_img)
display_values(speed, throttle, brake, steer, steering_wheel_img, screen)

# Joystick control
events = pygame.event.get()
steer_control = joystick.get_axis(0)
throttle_brake_control = joystick.get_axis(4)
steer_value = -int(steer_control * 270)
throttle_value = int(-min(0, throttle_brake_control) * 100)
brake_value = int(max(0, throttle_brake_control) * 100)
print(f"Steer: {steer_value}, Throttle: {throttle_value}, Brake: {brake_value}")

# Send control data
# steering
controller.message_to_send[0x102] = [
0x01, # enControl
0x00, # speed
0x00,
(steer_value + 500) >> 8,
(steer_value + 500) & 0xFF,
0x00,
0x00,
0x00,
]
# throttle
controller.message_to_send[0x100] = [
0x01, # enControl+
0x00,
0x00,
((throttle_value * 10) >> 8),
((throttle_value * 10) & 0xFF),
0x00,
0x00,
0x00,
]
# brake
controller.message_to_send[0x101] = [
0x01, # enControl
0x00,
0x00,
((brake_value * 10) >> 8),
((brake_value * 10) & 0xFF),
0x00,
0x00,
0x00,
]
controller.message_to_send[0x103] = [1,4,0,0,0,0,0,0] # gear
controller.message_to_send[0x104] = [0,0,0,0,0,0,0,0] # park
controller.message_to_send[0x105] = [0,0,0,0,0,0,0,0] # vehicle mode

time.sleep(0.1)
except KeyboardInterrupt:
controller.close()
Expand Down

0 comments on commit 451abf9

Please sign in to comment.