bendns' repos / ArduinoStuff
arduino stuffs
Diffstat (limited to 'car_driver/arduino/arduino.ino')
-rw-r--r--car_driver/arduino/arduino.ino65
1 files changed, 34 insertions, 31 deletions
diff --git a/car_driver/arduino/arduino.ino b/car_driver/arduino/arduino.ino
index 98e1b2c..37844a4 100644
--- a/car_driver/arduino/arduino.ino
+++ b/car_driver/arduino/arduino.ino
@@ -1,5 +1,4 @@
#define HEADER 'H'
-#define TOTAL_BYTES 3
const int MOVE_FORWARD[2] = {0, 2}; // 2 == -1 == up
const int MOVE_LEFT[2] = {2, 0}; // move left
@@ -10,46 +9,50 @@ const int MOVE_STOP[2] = {0, 0}; // stop
void setup() {
Serial.begin(9600);
Serial.println("initialized");
+ Serial.setTimeout(10);
begin();
- processCommand(0, 2);
}
void loop() {
- if (Serial.available() >= TOTAL_BYTES) {
- // sample format: H11 or H02, etc.
- char header = Serial.read();
- if (header == HEADER) { // done reading header, next number is a tag
- char tmp = Serial.read();
- int x = (tmp - '0');
- tmp = Serial.read();
- int y = (tmp - '0');
- processCommand(x, y); // process the command
+ if (Serial.available() > 0) {
+ String string = Serial.readString();
+ string.trim();
+ 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();
+ processCommand(x, y);
}
}
}
+#define DEADZONE 10
// @param cmd the thing that tells it what to do, usually a vector(01, 22).
void processCommand(const int x, const int y) {
- const int cmd[] = {x, y};
- if (arrayCmp(cmd, MOVE_FORWARD)) {
- forward();
- } else if (arrayCmp(cmd, MOVE_STOP)) {
- stop();
- } else if (arrayCmp(cmd, MOVE_LEFT)) {
- left();
- } else if (arrayCmp(cmd, MOVE_RIGHT)) {
- right();
- } else if (arrayCmp(cmd, MOVE_BACK)) {
- backward();
+ if (x > 100 || x < -100 || y > 100 || y < -100) {
+ Serial.println("invalid command");
+ }
+ if (x < DEADZONE && x >= 0) {
+ // apply brakes to motora
+ brakeMotorA();
+ } else if (x < 0) {
+ motorABackward(abs(x) + 155);
} else {
- Serial.print("( ");
- Serial.print(x);
- Serial.print(", ");
- Serial.print(y);
- Serial.println(" ) Ignored");
+ // positive number: 100 + 155 = 255
+ motorAForward(x + 155);
}
-}
-
-bool arrayCmp(const int array[], const int array2[]) {
- return array[0] == array2[0] && array[1] == array2[1];
+ if (y < DEADZONE && y >= 0) {
+ // apply brakes to motorb
+ brakeMotorB();
+ } else if (y < 0) { // negative number: -100 + 355 = 255
+ motorBBackward(abs(y) + 155);
+ } else {
+ // apply speed to motorb
+ motorBForward(y + 155);
+ }
+ Serial.print("x: ");
+ Serial.print(String(x));
+ Serial.print(" y: ");
+ Serial.println(String(y));
} \ No newline at end of file