bendns' repos / ArduinoStuff
arduino stuffs
Diffstat (limited to 'car_driver/arduino/arduino.ino')
-rw-r--r--car_driver/arduino/arduino.ino44
1 files changed, 25 insertions, 19 deletions
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