Mercurial > ukubik

changeset 76:a16884812b83

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Huge refactoring of the arduino code.
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Sun, 14 Sep 2014 13:12:28 +0200
parents d89650206f92
children c104b318ec34
files servo/accel.ino servo/battery.ino servo/beep.ino servo/legs.ino servo/servo.ino servo/servos.ino
diffstat 6 files changed, 224 insertions(+), 232 deletions(-) [+]
line wrap: on
line diff
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo/accel.ino	Sun Sep 14 13:12:28 2014 +0200
@@ -0,0 +1,53 @@
+// 3.3V maximum for the acceleration sensor.
+#define ACCEL_MAX 676
+
+// How many acceleration measurements to cache.
+#define MAX_ACCEL_BUFFER 16
+
+
+int accel_buffer[MAX_ACCEL_BUFFER][3];
+
+
+void read_accel() {
+    static unsigned char accel_buffer_index = 0;
+    accel_buffer[accel_buffer_index][0] = analogRead(A2);
+    accel_buffer[accel_buffer_index][1] = analogRead(A1);
+    accel_buffer[accel_buffer_index][2] = analogRead(A0);
+    accel_buffer_index += 1;
+    if (accel_buffer_index >= MAX_ACCEL_BUFFER) {
+        accel_buffer_index = 0;
+    }
+}
+
+void do_accel() {
+    /* Report the readings of the acceleration sensor. */
+    int avg_x = 0;
+    int avg_y = 0;
+    int avg_z = 0;
+    int min_x = 676;
+    int min_y = 676;
+    int min_z = 676;
+    int max_x = 0;
+    int max_y = 0;
+    int max_z = 0;
+    for (unsigned char i = 0; i < MAX_ACCEL_BUFFER; i += 1) {
+        for (unsigned char axe = 0; axe < 3; ++axe) {
+            avg_x += accel_buffer[i][axe];
+            min_x = min(min_x, accel_buffer[i][axe]);
+            max_x = max(max_x, accel_buffer[i][axe]);
+        }
+    }
+    avg_x /= MAX_ACCEL_BUFFER;
+    avg_y /= MAX_ACCEL_BUFFER;
+    avg_z /= MAX_ACCEL_BUFFER;
+    Serial.write(0xff);
+    Serial.write(map(avg_x, 0, 676, 0, 255));
+    Serial.write(map(avg_y, 0, 676, 0, 255));
+    Serial.write(map(avg_z, 0, 676, 0, 255));
+    Serial.write(map(min_x, 0, 676, 0, 255));
+    Serial.write(map(min_y, 0, 676, 0, 255));
+    Serial.write(map(min_z, 0, 676, 0, 255));
+    Serial.write(map(max_x, 0, 676, 0, 255));
+    Serial.write(map(max_y, 0, 676, 0, 255));
+    Serial.write(map(max_z, 0, 676, 0, 255));
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo/battery.ino	Sun Sep 14 13:12:28 2014 +0200
@@ -0,0 +1,24 @@
+
+void check_battery() {
+    static int battery_fail = 0;
+    int reading = analogRead(A7);
+    if (reading < 762) {
+        /* At about 3.74V start worrying. */
+        battery_fail += 1;
+    } else {
+        battery_fail = max(0, battery_fail - 1);
+    }
+    if (battery_fail > 10) {
+        /* If it doesn't improve for a second, beep. */
+        tone(A3, 880, 100);
+    }
+    if (battery_fail > 40) {
+        /* If it stays bad for 4s, shut down. */
+        fold();
+    }
+}
+
+void do_battery() {
+    Serial.write(0xff);
+    Serial.write(map(analogRead(A7), 0, 1023, 0, 255));
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo/beep.ino	Sun Sep 14 13:12:28 2014 +0200
@@ -0,0 +1,17 @@
+
+void beep_setup() {
+    pinMode(A3, OUTPUT);
+    delay(50);
+    tone(A3, 440, 200);
+    delay(200);
+    tone(A3, 523, 100);
+    delay(150);
+    tone(A3, 880, 100);
+}
+
+void do_beep() {
+    /* Emit a beep of specified frequency and duration. */
+    int frequency = _serial_read_word();
+    int duration = Serial.read();
+    tone(A3, frequency, duration * 10);
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo/legs.ino	Sun Sep 14 13:12:28 2014 +0200
@@ -0,0 +1,27 @@
+// Leg dimensions.
+#define LEG_COXA 22.5
+#define LEG_FEMUR 38.0
+#define LEG_TIBIA 30.0
+#define LEG_BASE 15.0
+
+const unsigned char LEG_HIP[4] = {11, 8, 9, 10};
+const unsigned char LEG_KNEE[4] = {4, 7, 6, 5};
+const unsigned char LEG_ANKLE[4] = {0, 3, 2, 1};
+const char LEG_X[4] = {-1, -1, 1, 1};
+const char LEG_Y[4] = {-1, 1, 1, -1};
+
+
+void fold() {
+    /* Folds the robots and shut downs all servos. Doesn't return. */
+    for (int leg = 0; leg < 4; leg += 1) {
+        int d = LEG_X[leg] * LEG_X[leg];
+        servo_degrees(LEG_HIP[leg], 90 + 45 * d);
+        servo_degrees(LEG_KNEE[leg], 90 - 90 * d);
+        servo_degrees(LEG_ANKLE[leg], 90 + 90 * d);
+    }
+    delay(250);
+    servo_shutdown();
+    while (1) {
+        delay(1000);
+    }
+}
--- a/servo/servo.ino	Thu Sep 11 18:17:21 2014 +0200
+++ b/servo/servo.ino	Sun Sep 14 13:12:28 2014 +0200
@@ -1,192 +1,17 @@
-/*
-A very simple servo controller code.
-*/
-
-#include <Servo.h>
-
 // Time of a signle frame, in milliseconds.
 #define FRAME_TIME 10
 
-// Number of servos in legs.
-#define MAX_SERVO 12
-
-// Range of the servos.
-#define SERVO_FREQ_MIN 600
-#define SERVO_FREQ_MAX 2400
-#define SERVO_PIN_MIN 2
-
-// 3.3V maximum for the acceleration sensor.
-#define ACCEL_MAX 676
-
-// How many acceleration measurements to cache.
-#define MAX_ACCEL_BUFFER 16
-
 // Speed of the serial port.
 #define SERIAL_SPEED 115200
 // 115200
 // 38400
 // 9600
 
-// Leg dimensions.
-#define LEG_COXA 22.5
-#define LEG_FEMUR 38.0
-#define LEG_TIBIA 30.0
-#define LEG_BASE 15.0
-
-
-int joints[12][2] {
-//  reverse, trim
-    // ankles
-    {1, -50},  // front right
-    {-1, 0}, // front left
-    {1, -50},   // hind left
-    {-1, -50},  // hind right
-
-    // knees
-    {1, -70},
-    {-1, 50},
-    {1, 30},
-    {-1, 50},
-
-    // hips
-    {-1, 0},
-    {1, 0},
-    {1, 0},
-    {-1, 0},
-};
-int legs[4][5] = {
-//  hip, knee, ankle, x_direction, y_direction
-    {11, 4, 0, -1, -1},  // front right
-    {8, 7, 3, -1, 1},  // front left
-    {9, 6, 2, 1, 1},  // hind left
-    {10, 5, 1, 1, -1},  // hind right
-};
-Servo servos[MAX_SERVO];
-int serial_command = 0;
-int accel_buffer[MAX_ACCEL_BUFFER][3];
-int accel_buffer_index = 0;
-int battery_fail = 0;
-unsigned long last_tick = 0;
-
-
-void read_accel() {
-    accel_buffer[accel_buffer_index][0] = analogRead(A2);
-    accel_buffer[accel_buffer_index][1] = analogRead(A1);
-    accel_buffer[accel_buffer_index][2] = analogRead(A0);
-    accel_buffer_index += 1;
-    if (accel_buffer_index >= MAX_ACCEL_BUFFER) {
-        accel_buffer_index = 0;
-    }
-}
-
-
-int _serial_read_word() {
-    /* Read two bytes from the serial port and make an int from them. */
-    int low;
-    int high;
-    low = Serial.read();
-    high = Serial.read();
-    return low | (high << 8);
-}
-
-void servo_write(int servo, int us) {
-    int pos = min(max(us + joints[servo][1], SERVO_FREQ_MIN), SERVO_FREQ_MAX);
-    if (joints[servo][0] < 0) {
-        pos = SERVO_FREQ_MAX + SERVO_FREQ_MIN - pos;
-    }
-    servos[servo].writeMicroseconds(pos);
-}
-
-
-void do_servo_degrees() {
-    /* Move a single servo by degrees. */
-    int servo = Serial.read();
-    int pos = Serial.read();
-    if (servo >= 0 && servo < MAX_SERVO && pos >= 0 && pos <= 180) {
-        servos[servo].write(pos);
-    }
-}
-
-void do_servos_degrees() {
-    /* Move all servos by degrees. */
-    for (int servo = 0; servo < MAX_SERVO; servo += 1) {
-        int pos = Serial.read();
-        if (pos >= 0 && pos <= 180) {
-            servos[servo].write(pos);
-        }
-    }
-}
-
-void do_servo_microseconds() {
-    /* Move a single servo by microseconds. */
-    int servo = Serial.read();
-    int pos = _serial_read_word();
-    if (servo >= 0 && servo < MAX_SERVO &&
-        pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
-        servo_write(servo, pos);
-    }
-}
-
-void do_servos_microseconds() {
-    /* Move all servos by microseconds. */
-    for (int servo = 0; servo < MAX_SERVO; servo += 1) {
-        int pos = _serial_read_word();
-        if (pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
-            servo_write(servo, pos);
-        }
-    }
-}
-
-void do_accel() {
-    /* Report the readings of the acceleration sensor. */
-    int avg_x = 0;
-    int avg_y = 0;
-    int avg_z = 0;
-    int min_x = 676;
-    int min_y = 676;
-    int min_z = 676;
-    int max_x = 0;
-    int max_y = 0;
-    int max_z = 0;
-    for (int i = 0; i < MAX_ACCEL_BUFFER; i += 1) {
-        avg_x += accel_buffer[i][0];
-        avg_y += accel_buffer[i][1];
-        avg_z += accel_buffer[i][2];
-        min_x = min(min_x, accel_buffer[i][0]);
-        min_y = min(min_y, accel_buffer[i][1]);
-        min_z = min(min_z, accel_buffer[i][2]);
-        max_x = max(max_x, accel_buffer[i][0]);
-        max_y = max(max_y, accel_buffer[i][1]);
-        max_z = max(max_z, accel_buffer[i][2]);
-    }
-    avg_x /= MAX_ACCEL_BUFFER;
-    avg_y /= MAX_ACCEL_BUFFER;
-    avg_z /= MAX_ACCEL_BUFFER;
-    Serial.write(0xff);
-    Serial.write(map(avg_x, 0, 676, 0, 255));
-    Serial.write(map(avg_y, 0, 676, 0, 255));
-    Serial.write(map(avg_z, 0, 676, 0, 255));
-    Serial.write(map(min_x, 0, 676, 0, 255));
-    Serial.write(map(min_y, 0, 676, 0, 255));
-    Serial.write(map(min_z, 0, 676, 0, 255));
-    Serial.write(map(max_x, 0, 676, 0, 255));
-    Serial.write(map(max_y, 0, 676, 0, 255));
-    Serial.write(map(max_z, 0, 676, 0, 255));
-}
-
-void do_beep() {
-    /* Emit a beep of specified frequency and duration. */
-    int frequency = _serial_read_word();
-    int duration = Serial.read();
-    tone(A3, frequency, duration * 10);
-}
-
-void do_battery() {
-    Serial.write(0xff);
-    Serial.write(map(analogRead(A7), 0, 1023, 0, 255));
-}
+// Total number of servos.
+#define MAX_SERVO 12
 
 void do_serial_command() {
+    static int serial_command = 0;
     /* Read and execute a command from the serial port's buffer. */
     switch (serial_command) {
         case 0x00:
@@ -232,65 +57,14 @@
     serial_command = 0;
 }
 
-void unfold() {
-    for (int servo = MAX_SERVO - 1; servo >= 0; servo -= 1) {
-        /* First 2 pins are used for the serial. */
-        servos[servo].attach(servo + SERVO_PIN_MIN,
-                             SERVO_FREQ_MIN, SERVO_FREQ_MAX);
-        servo_write(servo, 1500);
-        /* Don't set all servos at once, save power. */
-        delay(150);
-    }
-    delay(50);
-    tone(A3, 440, 200);
-    delay(200);
-    tone(A3, 523, 100);
-    delay(150);
-    tone(A3, 880, 100);
-}
-
-void fold() {
-    /* Folds the robots and shut downs all servos. Doesn't return. */
-    for (int leg = 0; leg < 4; leg += 1) {
-        int d = legs[leg][3] * legs[leg][4];
-        servos[legs[leg][0]].write(90 + 45 * d);
-        servos[legs[leg][1]].write(90 - 90 * d);
-        servos[legs[leg][2]].write(90 + 90 * d);
-    }
-    delay(250);
-    for (int servo = 0; servo < MAX_SERVO; servo += 1) {
-        servos[servo].detach();
-    }
-    while (1) {
-        delay(1000);
-    }
-}
-
-void check_battery() {
-    int reading = analogRead(A7);
-    if (reading < 762) {
-        /* At about 3.74V start worrying. */
-        battery_fail += 1;
-    } else {
-        battery_fail = max(0, battery_fail - 1);
-    }
-    if (battery_fail > 10) {
-        /* If it doesn't improve for a second, beep. */
-        tone(A3, 880, 100);
-    }
-    if (battery_fail > 40) {
-        /* If it stays bad for 4s, shut down. */
-        fold();
-    }
-}
-
 void setup() {
     Serial.begin(SERIAL_SPEED);
-    pinMode(A3, OUTPUT);
-    unfold();
+    servo_setup();
+    beep_setup();
 }
 
 void loop() {
+    static unsigned long last_tick = 0;
     unsigned long now = millis();
 
     do_serial_command();
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servo/servos.ino	Sun Sep 14 13:12:28 2014 +0200
@@ -0,0 +1,97 @@
+#include <Servo.h>
+
+
+// Range of the servos.
+#define SERVO_FREQ_MIN 600
+#define SERVO_FREQ_MAX 2400
+
+const unsigned char SERVO_PIN[MAX_SERVO] = {
+    2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13
+};
+const char SERVO_REVERSE[MAX_SERVO] = {
+    1, -1, 1, -1, 1, -1, 1, -1, -1, 1, 1, -1
+};
+const char SERVO_TRIM[MAX_SERVO] = {
+    -50, 0, -50, -50, -70, 50, 30, 50, 0, 0, 0, 0
+};
+const double US_PER_DEG = double(SERVO_FREQ_MAX - SERVO_FREQ_MIN) / 180;
+
+Servo servos[MAX_SERVO];
+
+
+void servo_write(unsigned char servo, unsigned int us) {
+    int pos = min(max(us + SERVO_TRIM[servo], SERVO_FREQ_MIN), SERVO_FREQ_MAX);
+    if (SERVO_REVERSE[servo] < 0) {
+        pos = SERVO_FREQ_MAX + SERVO_FREQ_MIN - pos;
+    }
+    servos[servo].writeMicroseconds(pos);
+}
+
+void servo_degrees(unsigned char servo, double angle) {
+    servo_write(servo, SERVO_FREQ_MIN + angle * US_PER_DEG);
+}
+
+void servo_setup() {
+    for (int servo = MAX_SERVO - 1; servo >= 0; servo -= 1) {
+        /* First 2 pins are used for the serial. */
+        servos[servo].attach(SERVO_PIN[servo], SERVO_FREQ_MIN, SERVO_FREQ_MAX);
+        servo_write(servo, 1500);
+        /* Don't set all servos at once, save power. */
+        delay(150);
+    }
+}
+
+void servo_shutdown() {
+    for (int servo = 0; servo < MAX_SERVO; servo += 1) {
+        servos[servo].detach();
+    }
+}
+
+int _serial_read_word() {
+    /* Read two bytes from the serial port and make an int from them. */
+    int low;
+    int high;
+    low = Serial.read();
+    high = Serial.read();
+    return low | (high << 8);
+}
+
+
+void do_servo_degrees() {
+    /* Move a single servo by degrees. */
+    int servo = Serial.read();
+    int pos = Serial.read();
+    if (servo >= 0 && servo < MAX_SERVO && pos >= 0 && pos <= 180) {
+        servos[servo].write(pos);
+    }
+}
+
+void do_servos_degrees() {
+    /* Move all servos by degrees. */
+    for (int servo = 0; servo < MAX_SERVO; servo += 1) {
+        int pos = Serial.read();
+        if (pos >= 0 && pos <= 180) {
+            servos[servo].write(pos);
+        }
+    }
+}
+
+void do_servo_microseconds() {
+    /* Move a single servo by microseconds. */
+    int servo = Serial.read();
+    int pos = _serial_read_word();
+    if (servo >= 0 && servo < MAX_SERVO &&
+        pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
+        servo_write(servo, pos);
+    }
+}
+
+void do_servos_microseconds() {
+    /* Move all servos by microseconds. */
+    for (int servo = 0; servo < MAX_SERVO; servo += 1) {
+        int pos = _serial_read_word();
+        if (pos >= SERVO_FREQ_MIN && pos <= SERVO_FREQ_MAX) {
+            servo_write(servo, pos);
+        }
+    }
+}