ArduinoStuff

diff --git a/car_driver/.clang-format b/car_driver/.clang-format
deleted file mode 100644
index 0296909..0000000
--- a/car_driver/.clang-format
+++ /dev/null

@@ -1,2 +0,0 @@

-"BreakBeforeBraces": Attach \ No newline at end of file

diff --git a/car_driver/arduino/arduino.ino b/car_driver/arduino/arduino.ino
index e6617eb..3150ea8 100644
--- a/car_driver/arduino/arduino.ino
+++ b/car_driver/arduino/arduino.ino

@@ -6,61 +6,50 @@ 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;

+int8_t prev_a = 0, prev_b = 0;

+bool isserial = false;

#include "Streaming.h"

#include "drive.h"

+#include "infared.h"

void setup() {

Serial.begin(9600);

- Serial.println("initialized");

+ Infared::begin();

+ Serial.println(F("initialized"));

Serial.setTimeout(10);

begin();

}

void loop() {

+ int8_t a = 255, b = 255;

if (Serial.available() > 0) {

+ isserial = true;

String string = Serial.readString();

string.trim();

char header = string[0];

if (header == HEADER) {

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

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

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

- processCommand(x, y);

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

+ b = string.substring(delimiter + 1).toInt();

}

+ } else if (not isserial) {

+ Infared::loop();

+ a = Infared::motor_a();

+ b = Infared::motor_b();

}

+ if (a != 255 and b != 255)

+ processCommand(a, b);

}

// @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)

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

+ Serial << F("invalid command") << a << ',' << b << endl;

return;

+ }

prev_a = a, prev_b = b;

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

- Serial << "invalid command"

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

- }

- if (a == 0) {

- // apply brakes to motora

- motor_a.brake();

- } else if (a < 0) {

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

- } else {

- // positive number: 100 + 155 = 255

- motor_a.forward(a + 155);

- }

- if (b == 0) {

- // apply brakes to motorb

- motor_b.brake();

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

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

- } else {

- // apply speed to motorb

- motor_b.forward(b + 155);

- }

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

+ speed(a, b);

} \ No newline at end of file

diff --git a/car_driver/arduino/drive.h b/car_driver/arduino/drive.h
index 89a42b3..c60129d 100644
--- a/car_driver/arduino/drive.h
+++ b/car_driver/arduino/drive.h

@@ -3,8 +3,7 @@

#include "motor.h"

-Motor motor_b(13, 8, 11);

-Motor motor_a(12, 9, 3);

+Motor motor_a(12, 9, 3), motor_b(13, 8, 11);

inline void begin() {

motor_a.begin();

diff --git a/car_driver/arduino/infared.h b/car_driver/arduino/infared.h
new file mode 100644
index 0000000..b445561
--- /dev/null
+++ b/car_driver/arduino/infared.h

@@ -0,0 +1,140 @@

+/*

+ * IRremote: IRrecvDemo - demonstrates receiving IR codes with IRrecv

+ * An IR detector/demodulator must be connected to the input RECV_PIN.

+ * Version 0.1 July, 2009

+ * http://arcfn.com

+ */

+#include <IRremote.h>

+#include <Streaming.h>

+namespace Infared {

+#define RECV_PIN 2

+struct {

+} ir_map;

+#define NO_INPUT 0x00

+#define ESTOP 0xFFF807

+#define STOP 0xFF32CD

+#define UP 0xFF0AF5

+#define LEFT 0xFF22DD

+#define RIGHT 0xFF1AE5

+#define DOWN 0xFFF00F

+#define UP_LEFT 0xFF12ED

+#define UP_RIGHT 0xFF2AD5

+#define DOWN_LEFT 0xFFE01F

+#define DOWN_RIGHT 0xFF9A65

+#pragma region math

+inline void lerp(float &from, const float to, const float weight) { from = from + (to - from) * weight; }

+inline float lerpr(const float from, const float to, const float weight) { return from + (to - from) * weight; }

+static int clamp(long value, long minv, long maxv) {

+ if (value < minv)

+ return minv;

+ else if (value > maxv)

+ return maxv;

+ return value;

+void kinda_round(float &n) {

+ if (n > .9)

+ n = 1;

+ else if (n < -.9)

+ n = -1;

+ else if (n > -.1 and n < .1)

+ n = 0;

+inline int8_t mult(const float n) { return round(clamp(n * 100, -100, 100)); }

+#pragma endregion

+int8_t motors[2] = {0, 0};

+void map(const unsigned long cmd) {

+ static float forces = 0, torque = 0;

+ const float large_weight = .8, weight = .5, low_weight = .3, speed = 1;

+ switch (cmd) {

+ case ESTOP:

+ torque = 0;

+ forces = 0;

+ case NO_INPUT:

+ lerp(forces, 0, large_weight);

+ lerp(torque, 0, large_weight);

+ break;

+ case UP:

+ lerp(forces, +speed, weight);

+ break;

+ case LEFT:

+ lerp(torque, +speed, weight);

+ break;

+ case RIGHT:

+ lerp(torque, -speed, weight);

+ break;

+ case UP_LEFT:

+ lerp(forces, +speed, low_weight);

+ lerp(torque, -speed, low_weight);

+ break;

+ case UP_RIGHT:

+ lerp(forces, +speed, low_weight);

+ lerp(torque, +speed, low_weight);

+ break;

+ case DOWN:

+ lerp(forces, -speed, weight);

+ break;

+ case DOWN_LEFT:

+ lerp(forces, -speed, low_weight);

+ lerp(torque, -speed / 2, low_weight);

+ break;

+ case DOWN_RIGHT:

+ lerp(forces, -speed, low_weight);

+ lerp(torque, +speed / 2, low_weight);

+ break;

+ default:

+ break;

+ }

+ kinda_round(forces);

+ kinda_round(torque);

+ const float turn_amount = abs(torque) / 2.5;

+ const int8_t a = mult(lerpr(forces, torque, turn_amount)), b = mult(lerpr(forces, -torque, turn_amount));

+ if (forces > 0) {

+ motors[0] = a;

+ motors[1] = b;

+ } else {

+ motors[1] = a;

+ motors[0] = b;

+ }

+IRrecv irrecv(RECV_PIN);

+decode_results results;

+unsigned long last_result_time = 0, last_real_result = 0;

+void begin() {

+ Serial.begin(9600);

+ irrecv.enableIRIn(); // Start the receiver

+void loop() {

+ if (irrecv.decode(&results)) {

+ if (results.value == REPEAT)

+ results.value = last_real_result;

+ else

+ last_real_result = results.value;

+ map(results.value);

+ last_result_time = millis();

+ irrecv.resume();

+ } else if (not(motors[0] == 0 and motors[1] == 0) and millis() - last_result_time > 200)

+ // no results in a while (.2 seconds), lerpit down

+ map(NO_INPUT);

+inline int8_t motor_a() { return motors[0]; }

+inline int8_t motor_b() { return motors[1]; }

+} // namespace Infared \ No newline at end of file

diff --git a/car_driver/arduino/motor.h b/car_driver/arduino/motor.h
index cc57f72..5d7b6e1 100644
--- a/car_driver/arduino/motor.h
+++ b/car_driver/arduino/motor.h

@@ -3,9 +3,7 @@

class Motor {

private:

- int pin;

- int brake_pin;

- int speed_pin;

+ uint8_t pin, brake_pin, speed_pin;

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

// @param speed 0-255

@@ -39,11 +37,8 @@ public:

pinMode(brake_pin, OUTPUT);

}

- 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;

+ 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;

}

};

#endif \ No newline at end of file

diff --git a/car_driver/godot/car_driver/ConsoleWindow.gd b/car_driver/godot/car_driver/ConsoleWindow.gd
index a37106a..5202f08 100644
--- a/car_driver/godot/car_driver/ConsoleWindow.gd
+++ b/car_driver/godot/car_driver/ConsoleWindow.gd

@@ -15,5 +15,5 @@ func _process(_delta := 0.0):

func _on_Send_text_entered(new_text: String):

new_text = new_text.strip_edges().strip_escapes()

if new_text:

- SerialIO.send(new_text)

+ SerialIO.write(new_text)

send.text = ""

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

@@ -18,9 +18,8 @@ func write(motors: PoolIntArray) -> void:

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))]

+ var turn_amount := abs(torque) / 2.5

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

if force < 0:

input.invert()

return input