e

camera_control.py

@@ -0,0 +1,19 @@
+import cv2
+
+cap = cv2.VideoCapture(0)
+
+# Check if the webcam is opened correctly
+if not cap.isOpened():
+    raise IOError("Cannot open webcam")
+
+while True:
+    ret, frame = cap.read()
+    frame = cv2.resize(frame, None, fx=0.5, fy=0.5, interpolation=cv2.INTER_AREA)
+    cv2.imshow('Input', frame)
+
+    c = cv2.waitKey(1)
+    if c == 27: #Esc
+        break
+
+cap.release()
+cv2.destroyAllWindows()

embedded/goal_detector/goal_detector.ino

@@ -0,0 +1,43 @@
+#define OUT_ROBOT 6
+#define IN_ROBOT 8
+
+#define OUT_PLAYER 7
+#define IN_PLAYER 9
+
+int input_value_player;
+int input_value_robot;
+const int score_threshold_robot = 750;
+const int score_threshold_player = 950;
+int cycles = 0;
+
+void setup() {
+Serial.begin(115200);
+pinMode(OUT_ROBOT, OUTPUT);
+pinMode(OUT_PLAYER, OUTPUT);
+pinMode(IN_ROBOT, INPUT);
+pinMode(IN_PLAYER, INPUT);
+digitalWrite(OUT_PLAYER, HIGH);
+digitalWrite(OUT_ROBOT, HIGH);
+
+}
+
+void loop() {
+  cycles++;
+//  delay(5);
+  input_value_player = analogRead(IN_PLAYER);
+  input_value_robot = analogRead(IN_ROBOT);
+//  Serial.println(input_value_player);
+//  Serial.println(input_value_robot);
+  if (input_value_player < score_threshold_player) {
+    Serial.println("***ROBOT SCORED***");
+    delay(500);
+  } else if (input_value_robot < score_threshold_robot) {
+    Serial.println("***PLAYER SCORED***");
+    delay(500);
+  } else {
+    if (cycles > 2000) {
+          Serial.println("..."); 
+          cycles = 0;
+    }
+  }
+}

latency.py

@@ -0,0 +1,26 @@
+import time
+from pygame_control import Odrive
+
+
+
+drive = Odrive()
+print()
+print('sending drive in 1 sec')
+time.sleep(1)
+print('NOW')
+drive.goto(0.09, 3.255)
+print('5...')
+time.sleep(1)
+print('4...')
+time.sleep(1)
+print('3...')
+time.sleep(1)
+print('2...')
+time.sleep(1)
+print('1...')
+time.sleep(1)
+# for i in range(50):
+print('@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@')
+drive.goto(-0.8, 2.29)
+time.sleep(1)
+drive.set_idle()

odrive_server_main.py

@@ -0,0 +1,101 @@
+import odrive
+from odrive.enums import *
+import time
+import socket
+import atexit
+
+class Odrive:
+    def __init__(self) -> None:
+        self.drive = odrive.find_any()
+        time.sleep(1)
+        self.init_odrive()
+        self.set_zero()
+        self.set_closed_loop()
+
+    def init_odrive(self) -> None:
+        self.drive.axis0.controller.config.input_mode = 1
+        self.drive.axis1.controller.config.input_mode = 1
+        self.drive.axis0.motor.config.current_lim = 70
+        self.drive.axis1.motor.config.current_lim = 70
+        self.drive.axis0.motor.config.current_lim_margin = 30
+        self.drive.axis1.motor.config.current_lim_margin = 30
+        self.drive.axis0.controller.config.vel_limit = 10
+        self.drive.axis1.controller.config.vel_limit = 10
+
+
+    def set_zero(self) -> None:
+        self.drive.axis0.encoder.set_linear_count(0)
+        self.drive.axis1.encoder.set_linear_count(0)
+
+    def set_idle(self) -> None:
+        self.drive.axis0.requested_state = AXIS_STATE_IDLE
+        self.drive.axis1.requested_state = AXIS_STATE_IDLE
+
+    def set_closed_loop(self) -> None:
+        self.drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+        self.drive.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+        time.sleep(1)
+
+    def goto_raw(self, mot_0, mot_1) -> None:
+        self.drive.axis0.controller.input_pos = mot_0
+        self.drive.axis1.controller.input_pos = mot_1
+
+
+class Server:
+    def __init__(self) -> None:
+        self.armed = False
+        self.armed = True
+        self.x_offset = 0
+        self.x_multiplier = -98/1.1 # -98/10  # NOTE: Remove the /10
+        self.y_offset = 0
+        self.y_multiplier = 85/1.1 # 85/10  # NOTE: Remove the /10
+        self.sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
+        self.udp_host = socket.gethostname()
+        self.udp_port = 59200
+        self.sock.bind((self.udp_host, self.udp_port))
+        self.init_odrive()
+        atexit.register(self.at_exit)
+        self.control_loop()
+
+    def init_odrive(self) -> None:
+        self.drive = Odrive()
+
+    def control_loop(self) -> None:
+        while True:
+            try:
+                data_encoded, _ = self.sock.recvfrom(1024)
+                data_dirty = data_encoded.decode()
+                data_clean = data_dirty.strip('(').strip(')').split(',')
+                target = [0, 0]
+                target[0] = float(data_clean[0])
+                target[1] = float(data_clean[1])
+                if target[0] > 1 or target[0] < 0 or target[1] > 1 or target[1] < 0:
+                    print(f"{target} outside 0 to 1 range!")
+                    exit(2)
+                print(f"Received Message: {data_clean}, type: {type(data_clean)}")
+                print(f"Parsed Message: {target}, type: {type(target)}")
+                angle_0, angle_1 = self.calculate_angles(target[0], target[1])
+                print(f"angles: {angle_0}, {angle_1}")
+                if self.armed:
+                    self.drive.goto_raw(angle_0, angle_1)
+            except Exception as e:
+                print(f"Exception: {e}")
+                self.drive.set_idle()
+                exit(1)
+
+    def calculate_angles(self, target_pos_x, target_pos_y) -> float:
+        x = (target_pos_x + self.x_offset) * self.x_multiplier
+        y = (target_pos_y + self.y_offset) * self.y_multiplier
+        pi = 3.142
+        pulley_diameter = 10.0  # cm
+        distance_per_revolution = pi * pulley_diameter
+
+        target_angle_0 = (y / distance_per_revolution) - (x / distance_per_revolution)
+        target_angle_1 = (y / distance_per_revolution) + (x / distance_per_revolution)
+
+        return target_angle_0, target_angle_1
+
+    def at_exit(self):
+        self.drive.set_idle()
+
+s = Server()

odrive_setup.py

@@ -48,7 +48,7 @@ logging.warning(f"Axis 1 encoder bandwidth: {odrv0.axis1.encoder.bandwidth}")
 
 def controller_setup(odrv0: object, pos_gain: int = 1, vel_limit: int = 0.5):  # Low vel limit (1 turn every 2s to avoid crsahing lol)
     # https://docs.oodrv0robotics.com/control
-    odrv0.axis0.controller.config.pos_gain = pos_gain
+    odrv0.axis0.controller.config.pos_gain = pos_gain # 20
     odrv0.axis1.controller.config.pos_gain = pos_gain
     logging.warning(f'Set positional gain to {pos_gain}')
     ### NOTE: The VEL gain determines how fast the motor will move from point A to point B, may need to up/ tweak the value
@@ -182,3 +182,36 @@ odrv0.axis1.controller.input_pos = 0
 
 odrv0.axis0.encoder.set_linear_count(0)
 odrv0.axis1.encoder.set_linear_count(0)
+
+
+# defaults:
+# gain_scheduling_width = 10.0 (float)
+# enable_vel_limit = True (bool)
+# enable_current_mode_vel_limit = True (bool)
+# enable_gain_scheduling = False (bool)
+# enable_overspeed_error = True (bool)
+# control_mode = 3 (int)
+# input_mode = 1 (int)
+# pos_gain = 50.0 (float)
+# vel_gain = 0.1666666716337204 (float)
+# vel_integrator_gain = 0.3333333432674408 (float)
+# vel_limit = 100.0 (float)
+# vel_limit_tolerance = 2.0 (float)
+# vel_ramp_rate = 1.0 (float)
+# torque_ramp_rate = 0.009999999776482582 (float)
+# circular_setpoints = False (bool)
+# circular_setpoint_range = 1.0 (float)
+# homing_speed = 0.25 (float)
+# inertia = 0.0 (float)
+# axis_to_mirror = 255 (int)
+# mirror_ratio = 1.0 (float)
+# load_encoder_axis = 0 (int)
+# input_filter_bandwidth = 2.0 (float)
+# anticogging:
+#   index = 0 (int)
+#   pre_calibrated = False (bool)
+#   calib_anticogging = False (bool)
+#   calib_pos_threshold = 1.0 (float)
+#   calib_vel_threshold = 1.0 (float)
+#   cogging_ratio = 1.0 (float)
+#   anticogging_enabled = True (bool)

pygame_control.py

@@ -13,6 +13,23 @@ class Odrive:
     def set_zero(self):
         self.drive.axis0.encoder.set_linear_count(0)
         self.drive.axis1.encoder.set_linear_count(0)
+        self.drive.axis0.controller.config.input_mode = 1
+        self.drive.axis1.controller.config.input_mode = 1
+        # self.drive.axis0.trap_traj.config.vel_limit = 100
+        # self.drive.axis1.trap_traj.config.vel_limit = 100
+        # self.drive.axis0.trap_traj.config.accel_limit = 50
+        # self.drive.axis1.trap_traj.config.accel_limit = 50
+        # self.drive.axis0.trap_traj.config.decel_limit = 50
+        # self.drive.axis1.trap_traj.config.decel_limit = 50
+        # self.drive.axis0.controller.config.inertia = 0.02
+        # self.drive.axis1.controller.config.inertia = 0.02
+        self.drive.axis0.controller.config.input_filter_bandwidth = 30
+        self.drive.axis1.controller.config.input_filter_bandwidth = 30
+        self.drive.axis0.motor.config.current_lim = 65
+        self.drive.axis1.motor.config.current_lim = 65
+        self.drive.axis0.motor.config.current_lim_margin = 30
+        self.drive.axis1.motor.config.current_lim_margin = 30
+
 
     def set_idle(self):
         self.drive.axis0.requested_state = AXIS_STATE_IDLE
@@ -51,6 +68,10 @@ class Controller:
         self.a0_t = 0
         self.a1_t = 0
         self.v_max = self.drive.get_max_speed()
+        self.y_multiplier = 1.05
+        self.x_multiplier = 1.95
+        self.x_offset = 980
+        # self.y_multiplier = 0.7
 
     def calculate_angles(self, large_x, large_y):
         x = large_x / 10.0
@@ -92,7 +113,7 @@ class Controller:
             idkwhatthisis = self.font.render(f"X: {mouse_x} | Y: {500 - mouse_y}", True, (255, 255, 255))
             idkwhatthisis_3 = self.font.render(f"X: {mouse_x/100} | Y: {(500 - mouse_y)/100}", True, (255, 255, 255))
             idkwhatthisis_4 = self.font.render(f"M0: {self.a0:.3f} | M1: {self.a1:.3f}", True, (255, 255, 255))
-            self.a0_t, self.a1_t = self.calculate_angles(mouse_y*1.1, 990 - mouse_x)
+            self.a0_t, self.a1_t = self.calculate_angles(mouse_y*self.y_multiplier, 990 - mouse_x*2)
             idkwhatthisis_5 = self.font.render(f"M0: {self.a0_t:.3f} | M1: {self.a1_t:.3f}", True, (0, 255, 255))
             idkwhatthisis_6 = self.font.render(f"VMAX: {self.v_max}", True, (255, 100, 0))
             self.window.blit(idkwhatthisis, (0, 0))
@@ -104,7 +125,7 @@ class Controller:
             pygame.display.update()
 
             if self.armed:
-                self.a0, self.a1 = self.calculate_angles(mouse_y*1.1, 990 - mouse_x*2)
+                self.a0, self.a1 = self.calculate_angles(self.x_offset - mouse_x * self.x_multiplier, mouse_y*self.y_multiplier)
                 self.drive.goto(self.a0, self.a1)
 
         self.drive.set_idle()

scaled_pygame_control.py

@@ -0,0 +1,117 @@
+import pygame
+import odrive
+from odrive.enums import *
+import time
+
+class Odrive:
+    def __init__(self) -> None:
+        self.drive = odrive.find_any()
+        time.sleep(1)
+        self.set_zero()
+        self.set_closed_loop()
+
+    def set_zero(self):
+        self.drive.axis0.encoder.set_linear_count(0)
+        self.drive.axis1.encoder.set_linear_count(0)
+        self.drive.axis0.controller.config.input_mode = 1
+        self.drive.axis1.controller.config.input_mode = 1
+        self.drive.axis0.controller.config.input_filter_bandwidth = 30
+        self.drive.axis1.controller.config.input_filter_bandwidth = 30
+        self.drive.axis0.motor.config.current_lim = 65
+        self.drive.axis1.motor.config.current_lim = 65
+        self.drive.axis0.motor.config.current_lim_margin = 30
+        self.drive.axis1.motor.config.current_lim_margin = 30
+
+    def set_idle(self):
+        self.drive.axis0.requested_state = AXIS_STATE_IDLE
+        self.drive.axis1.requested_state = AXIS_STATE_IDLE
+
+    def set_closed_loop(self):
+        self.drive.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+        self.drive.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
+        time.sleep(1)
+
+    def goto(self, mot_0, mot_1):
+        self.drive.axis0.controller.input_pos = mot_0
+        self.drive.axis1.controller.input_pos = mot_1
+
+
+class Controller:
+    def __init__(self) -> None:
+        self.drive = Odrive()
+        pygame.init()
+        pygame.font.init()
+        self.font_name = pygame.font.get_default_font()
+        self.font = pygame.font.Font(self.font_name, 20)
+        self.window = pygame.display.set_mode((500, 500))
+        pygame.display.set_caption("RAH Visual Controller V0.1")
+        self.run = True
+        self.armed = False
+        self.prev_x = 0
+        self.prev_y = 0
+        self.a0 = 0
+        self.a1 = 0
+
+        self.x_offset = 0  #TODO: figure out what these are...
+        self.x_multiplier = -98  
+        self.y_offset = 0  #TODO: figure out what these are...
+        self.y_multiplier = 85  
+        
+
+    def calculate_angles(self, target_pos_x, target_pos_y) -> float:
+        x = (target_pos_x + self.x_offset) * self.x_multiplier
+        y = (target_pos_y + self.y_offset) * self.y_multiplier
+        pi = 3.142
+        pulley_diameter = 10.0  # cm
+        distance_per_revolution = pi * pulley_diameter
+
+        target_angle_0 = (y / distance_per_revolution) - (x / distance_per_revolution)
+        target_angle_1 = (y / distance_per_revolution) + (x / distance_per_revolution)
+
+        return target_angle_0, target_angle_1
+
+    def start(self):
+        while self.run:
+            pygame.time.delay(20)
+            for event in pygame.event.get():
+                if event.type == pygame.QUIT:
+                    self.run = False
+
+            mouse_x, mouse_y = pygame.mouse.get_pos()
+            keys = pygame.key.get_pressed()
+
+            if keys[pygame.K_SPACE]:
+                self.armed = True
+                self.window.fill((64, 0, 0))
+                idkwhatthisis_2 = self.font.render(f"Armed: {self.armed}", True, (255, 0, 0))
+                self.prev_x = mouse_x
+                self.prev_y = mouse_y
+            else:
+                self.armed = False
+                self.window.fill((10, 10, 10))
+                pygame.draw.circle(self.window, (255, 0, 0), (self.prev_x, self.prev_y), (20))
+                idkwhatthisis_2 = self.font.render(f"Armed: {self.armed}", True, (200, 200, 200))
+
+            pygame.draw.circle(self.window, (255, 0, 0), (mouse_x, mouse_y), (20))
+            pygame.draw.rect(self.window, (0, 255, 0), (mouse_x, 0, 1, 800))
+            pygame.draw.rect(self.window, (0, 128, 255), (0, mouse_y, 500, 1))
+            idkwhatthisis = self.font.render(f"X: {mouse_x} | Y: {mouse_y}", True, (255, 255, 255))
+            idkwhatthisis_3 = self.font.render(f"X: {mouse_x/500} | Y: {(mouse_y)/500}", True, (255, 255, 255))
+            idkwhatthisis_4 = self.font.render(f"M0: {self.a0:.3f} | M1: {self.a1:.3f}", True, (255, 255, 255))
+            self.window.blit(idkwhatthisis, (0, 0))
+            self.window.blit(idkwhatthisis_2, (0, 450))
+            self.window.blit(idkwhatthisis_3, (150, 0))
+            self.window.blit(idkwhatthisis_4, (200, 450))
+            pygame.display.update()
+
+            if self.armed:
+                self.a0, self.a1 = self.calculate_angles(mouse_x/500, mouse_y/500)
+                self.drive.goto(self.a0, self.a1)
+
+        self.drive.set_idle()
+        pygame.font.quit()
+        pygame.quit()
+
+if __name__ == "__main__":
+    c = Controller()
+    c.start()

stress_test.py

@@ -0,0 +1,27 @@
+import pygame
+import odrive
+from odrive.enums import *
+import time
+from pygame_control import Odrive
+
+points = [[1.147, 2.023], [1.397, 3.556], [-0.121, 3.214], [-0.642, 1.584]]
+
+class StressTest:
+    def __init__(self) -> None:
+        self.drive = Odrive()
+        self.drive.drive.axis0.controller.config.vel_limit = 30
+        self.drive.drive.axis1.controller.config.vel_limit = 30
+        self.drive.set_zero()
+        self.drive.set_idle()
+
+    def start(self):
+        self.drive.set_closed_loop()
+        while True:
+            print(f"FET0: {self.drive.drive.axis0.fet_thermistor.temperature}")
+            print(f"FET1: {self.drive.drive.axis1.fet_thermistor.temperature}")
+            for p in points:
+                self.drive.goto(p[0], p[1])
+                time.sleep(0.3)
+
+a = StressTest()
+a.start()