aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--robot/main.c35
-rw-r--r--robot/test/dummy.py51
2 files changed, 65 insertions, 21 deletions
diff --git a/robot/main.c b/robot/main.c
index 21d0e5c..a708a13 100644
--- a/robot/main.c
+++ b/robot/main.c
@@ -1,33 +1,26 @@
#include <pololu/orangutan.h>
#include <stdlib.h>
+unsigned char hex_to_int(char a) {
+ if (a >= 0x30 && a <= 0x39) return a - 0x30;
+ else if (a >= 0x61 && a <= 0x66) return a - 0x61 + 10;
+ return 0;
+}
+
int main() {
play("L50 c>c");
serial_set_baud_rate(9600);
- char *buf = malloc(20);
- unsigned int counter = 0;
-
+ char *buf = malloc(9);
while (1) {
- serial_receive_blocking(buf, 1, 65e3);
+ serial_receive_blocking(buf, 8, 65e3);
+
+ int modl = hex_to_int(buf[1]) - 1;
+ int speedl = hex_to_int(buf[2]) * 0x10 + hex_to_int(buf[3]);
+ int modr = hex_to_int(buf[5]) - 1;
+ int speedr = hex_to_int(buf[6]) * 0x10 + hex_to_int(buf[7]);
- switch (buf[0]) {
- case 0x7f: {
- counter--;
- lcd_goto_xy(counter, 0);
- print(" ");
- lcd_goto_xy(counter, 0);
- break;
- }
- default: {
- print(&buf[0]);
- counter++;
- if (counter > 20) {
- counter = 0;
- lcd_goto_xy(0, 0);
- }
- }
- }
+ set_motors(modl * speedl, modr * speedr);
}
return 0;
diff --git a/robot/test/dummy.py b/robot/test/dummy.py
new file mode 100644
index 0000000..5265fa0
--- /dev/null
+++ b/robot/test/dummy.py
@@ -0,0 +1,51 @@
+import pygame
+import serial
+pygame.init()
+joysticks = []
+clock = pygame.time.Clock()
+keepPlaying = True
+
+left_right_stick = 0
+forward_stick = 0
+backward_stick = 0
+
+def sign(num):
+ if num == 0:
+ return 0
+ elif num > 0:
+ return 1
+ else:
+ return -1
+
+serport = serial.Serial("/dev/ttyACM0", 9600, timeout=1)
+def send_speed(left, right):
+ total = ""
+ total += f"{(sign(left) + 1):02x}"
+ total += f"{(abs(left)):02x}"
+ total += f"{(sign(right) + 1):02x}"
+ total += f"{(abs(right)):02x}"
+
+ serport.write(bytes(total, 'utf-8'))
+ serport.flush()
+ print(total)
+
+for i in range(0, pygame.joystick.get_count()):
+ joysticks.append(pygame.joystick.Joystick(i))
+ joysticks[-1].init()
+ print ("Detected joystick "),joysticks[-1].get_name(),"'"
+
+def make_values():
+ left = 255 - max(0, left_right_stick) * 255
+ right = 255 + min(0, left_right_stick) * 255
+ speed_mod = backward_stick + forward_stick
+ return (int(left * speed_mod) , int(right * speed_mod))
+
+while keepPlaying:
+ clock.tick(60)
+ values = make_values()
+ send_speed(values[0], values[1])
+ for event in pygame.event.get():
+ if event.type != 1536: continue
+ if event.axis == 0: left_right_stick = event.value
+ if event.axis == 2: backward_stick = event.value * -0.5 - 1
+ if event.axis == 5: forward_stick = event.value * 0.5 + 1