diff options
| -rw-r--r-- | robot/main.c | 35 | ||||
| -rw-r--r-- | robot/test/dummy.py | 51 |
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 |