ArduinoStuff

diff --git a/car_driver/arduino/arduino.ino b/car_driver/arduino/arduino.ino
index 5be4b2d..e6617eb 100644
--- a/car_driver/arduino/arduino.ino
+++ b/car_driver/arduino/arduino.ino

@@ -6,6 +6,10 @@ const int MOVE_RIGHT[2] = {1, 0}; // move right

const int MOVE_BACK[2] = {0, 1}; // move back

const int MOVE_STOP[2] = {0, 0}; // stop

+int8_t prev_a = 0;

+int8_t prev_b = 0;

+#include "Streaming.h"

#include "drive.h"

void setup() {

@@ -22,39 +26,41 @@ void loop() {

char header = string[0];

if (header == HEADER) {

int delimiter = string.indexOf(',');

- int x = string.substring(1, delimiter).toInt();

- int y = string.substring(delimiter + 1).toInt();

+ int8_t x = string.substring(1, delimiter).toInt();

+ int8_t y = string.substring(delimiter + 1).toInt();

processCommand(x, y);

}

-#define DEADZONE 1

-// @param cmd the thing that tells it what to do, usually a vector(01, 22).

-void processCommand(const int x, const int y) {

- if (x > 100 || x < -100 || y > 100 || y < -100) {

- Serial.println("invalid command");

+// @param cmd the thing that tells it what to do, motor1, motorb

+void processCommand(const int8_t a, const int8_t b) {

+ if (a == prev_a || b == prev_b)

+ return;

+ prev_a = a, prev_b = b;

+ if (a > 100 || a < -100 || b > 100 || b < -100) {

+ Serial << "invalid command"

+ << "[" << a << "," << b << "]" << endl;

}

- if (x < DEADZONE && x >= 0) {

+ if (a == 0) {

// apply brakes to motora

motor_a.brake();

- } else if (x < 0) {

- motor_a.backward(abs(x) + 155);

+ } else if (a < 0) {

+ motor_a.backward(abs(a) + 155);

} else {

// positive number: 100 + 155 = 255

- motor_a.forward(x + 155);

+ motor_a.forward(a + 155);

}

- if (y < DEADZONE && y >= 0) {

+ if (b == 0) {

// apply brakes to motorb

motor_b.brake();

- } else if (y < 0) { // negative number: -100 + 355 = 255

- motor_b.backward(abs(y) + 155);

+ } else if (b < 0) { // negative number: -100 + 355 = 255

+ motor_b.backward(abs(b) + 155);

} else {

// apply speed to motorb

- motor_b.forward(y + 155);

+ motor_b.forward(b + 155);

}

- Serial.print("x: ");

- Serial.print(String(x));

- Serial.print(" y: ");

- Serial.println(String(y));

+ Serial << "a: " << a << " b: " << b << endl;

} \ No newline at end of file

diff --git a/car_driver/arduino/motor.h b/car_driver/arduino/motor.h
index f2f0f43..cfc05e4 100644
--- a/car_driver/arduino/motor.h
+++ b/car_driver/arduino/motor.h

@@ -8,10 +8,10 @@ struct Motor {

int brake_pin;

int speed_pin;

- inline void forward(int speed) {

+ inline void forward(int8_t speed) {

motor_utils::set(pin, brake_pin, speed_pin, HIGH, speed);

}

- inline void backward(int speed) {

+ inline void backward(int8_t speed) {

motor_utils::set(pin, brake_pin, speed_pin, LOW, speed);

}

inline void brake() { motor_utils::brake(brake_pin, speed_pin); }

@@ -21,7 +21,8 @@ struct Motor {

pinMode(brake_pin, OUTPUT);

}

- Motor(const int _pin, const int _brake_pin, const int _speed_pin) {

+ Motor(const uint8_t _pin, const uint8_t _brake_pin,

+ const uint8_t _speed_pin) {

pin = _pin;

brake_pin = _brake_pin;

speed_pin = _speed_pin;

diff --git a/car_driver/arduino/motor_utils.h b/car_driver/arduino/motor_utils.h
index fea6441..cb40b1b 100644
--- a/car_driver/arduino/motor_utils.h
+++ b/car_driver/arduino/motor_utils.h

@@ -4,13 +4,14 @@

namespace motor_utils {

// @param direction LOW is backwards, HIGH is forward

// @param speed 0-255

-inline void set(int motor, int brake, int speed_pin, int direction, int speed) {

- digitalWrite(motor, direction);

- digitalWrite(brake, LOW); // brake off

+inline void set(uint8_t motor_pin, uint8_t brake_pin, uint8_t speed_pin,

+ int8_t direction, uint8_t speed) {

+ digitalWrite(motor_pin, direction);

+ digitalWrite(brake_pin, LOW); // brake off

analogWrite(speed_pin, speed); // speed up

}

-inline void brake(int brake_pin, int speed_pin) {

+inline void brake(uint8_t brake_pin, uint8_t speed_pin) {

digitalWrite(brake_pin, HIGH); // engage brake

analogWrite(speed_pin, 0); // slow

}

diff --git a/car_driver/godot/car_driver/Input.gd b/car_driver/godot/car_driver/Input.gd
index 2275e9e..03a5187 100644
--- a/car_driver/godot/car_driver/Input.gd
+++ b/car_driver/godot/car_driver/Input.gd

@@ -1,36 +1,35 @@

extends Node

-var input := Vector2.ZERO

-var fast := false

-onready var lb = $Label

+onready var lb := $Label

+var prev_input: PoolIntArray

func _physics_process(_delta):

- var inp := get_input()

- if inp != input:

- input = inp

+ var input := get_input()

+ write(input) # write it multiple times because packet loss (idk either but it happens)

+ if prev_input != input:

+ prev_input = input

+ print(input)

lb.text = str(input)

- SerialIO.write("H%s,%s" % [input.x, input.y])

-func _ready():

- yield(get_tree(), "idle_frame")

- SerialIO.write("H0,0") #stop

- lb.text = str(input)

-func get_input() -> Vector2:

- if Input.is_action_just_pressed("sped"):

- fast = !fast

- var x := Input.get_action_strength("leftpaddle")

- var y := Input.get_action_strength("rightpaddle")

- var multiplier := 100 if fast else 50

- var v := Vector2(x, y) * multiplier

- # if button is pressed, reverse, if no paddle input, move slow

- if Input.is_action_pressed("lb"):

- v.x = -v.x if v.x else -multiplier / 4.0

- if Input.is_action_pressed("rb"):

- v.y = -v.y if v.y else -multiplier / 4.0

- return (v).round()

+func write(motors: PoolIntArray) -> void:

+ SerialIO.write("H%d,%d" % [motors[0], motors[1]])

+func get_input() -> PoolIntArray:

+ var force: float = Input.get_axis("decel", "accel")

+ var torque: float = Input.get_axis("ui_left", "ui_right")

+ var turn_sign := sign(torque)

+ var turn_amount := abs(torque)

+ var input: PoolIntArray = [mult(lerp(force, turn_sign, turn_amount)), mult(lerp(force, -turn_sign, turn_amount))]

+ if force < 0:

+ input.invert()

+ return input

+func mult(n: float) -> int:

+ return int(clamp(n * 100, -100, 100))

+func reset() -> void:

+ write([0, 0])

+func _exit_tree() -> void:

+ reset()

diff --git a/car_driver/godot/car_driver/project.godot b/car_driver/godot/car_driver/project.godot
index 0095f26..f1c1c61 100644
--- a/car_driver/godot/car_driver/project.godot
+++ b/car_driver/godot/car_driver/project.godot

@@ -45,14 +45,14 @@ singletons=[ ]

[input]

ui_left={

-"deadzone": 0.5,

+"deadzone": 0.2,

"events": [ Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":0,"alt":false,"shift":false,"control":false,"meta":false,"command":false,"pressed":false,"scancode":16777231,"physical_scancode":0,"unicode":0,"echo":false,"script":null)

, Object(InputEventJoypadButton,"resource_local_to_scene":false,"resource_name":"","device":0,"button_index":14,"pressure":0.0,"pressed":false,"script":null)

, Object(InputEventJoypadMotion,"resource_local_to_scene":false,"resource_name":"","device":0,"axis":0,"axis_value":-1.0,"script":null)

]

}

ui_right={

-"deadzone": 0.5,

+"deadzone": 0.2,

"events": [ Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":0,"alt":false,"shift":false,"control":false,"meta":false,"command":false,"pressed":false,"scancode":16777233,"physical_scancode":0,"unicode":0,"echo":false,"script":null)

, Object(InputEventJoypadButton,"resource_local_to_scene":false,"resource_name":"","device":0,"button_index":15,"pressure":0.0,"pressed":false,"script":null)

, Object(InputEventJoypadMotion,"resource_local_to_scene":false,"resource_name":"","device":0,"axis":0,"axis_value":1.0,"script":null)

@@ -97,6 +97,16 @@ sped={

"events": [ Object(InputEventJoypadButton,"resource_local_to_scene":false,"resource_name":"","device":0,"button_index":2,"pressure":0.0,"pressed":false,"script":null)

]

}

+accel={

+"deadzone": 0.2,

+"events": [ Object(InputEventJoypadMotion,"resource_local_to_scene":false,"resource_name":"","device":0,"axis":3,"axis_value":-1.0,"script":null)

+ ]

+decel={

+"deadzone": 0.2,

+"events": [ Object(InputEventJoypadMotion,"resource_local_to_scene":false,"resource_name":"","device":0,"axis":3,"axis_value":1.0,"script":null)

+ ]

[rendering]

diff --git a/car_driver/godot/car_driver/serial.gd b/car_driver/godot/car_driver/serial.gd
index 0d704fd..8931415 100644
--- a/car_driver/godot/car_driver/serial.gd
+++ b/car_driver/godot/car_driver/serial.gd

@@ -24,8 +24,7 @@ func create_serial():

func _ready():

create_serial()

-func _process(delta):

+func _process(_delta):

if serial.get_available()>0:

- var data = serial.read_string()

+ var data: String = serial.read_string()

emit_signal("recieved", data)

- print(data)