Mercurial > ukubik

changeset 60:1c45ce9bf382

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Better accelerometer readings
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Wed, 27 Aug 2014 11:24:40 +0200
parents 914f6c9612d7
children 0d85cebb5d41
files robot.py servo.py servo/servo.ino
diffstat 3 files changed, 41 insertions(+), 13 deletions(-) [+]
line wrap: on
line diff
--- a/robot.py	Wed Aug 27 10:58:30 2014 +0200
+++ b/robot.py	Wed Aug 27 11:24:40 2014 +0200
@@ -52,8 +52,12 @@
             leg.update(self.shift)
 
     def get_accel(self):
-        return [v - t for (v, t) in
-                zip(self.legs[0].hip.get_sensors(), self.accel_trim)]
+        sensors = self.legs[0].hip.get_sensors()
+        return [
+            [v - t for (v, t) in zip(sensors[0:3], self.accel_trim)],  # avg
+            [v - t for (v, t) in zip(sensors[3:6], self.accel_trim)],  # min
+            [v - t for (v, t) in zip(sensors[6:9], self.accel_trim)],  # max
+        ]
 
     def get_battery(self):
         return float(self.legs[0].hip.get_battery()) / self.battery_max
--- a/servo.py	Wed Aug 27 10:58:30 2014 +0200
+++ b/servo.py	Wed Aug 27 11:24:40 2014 +0200
@@ -44,7 +44,7 @@
         self.serial.write(command)
         while self.serial.read(1) != '\xff':
             pass
-        return [ord(c) for c in self.serial.read(3)]
+        return [ord(c) for c in self.serial.read(9)]
 
     def beep(self, frequency, duration):
         command = '\xf1'
--- a/servo/servo.ino	Wed Aug 27 10:58:30 2014 +0200
+++ b/servo/servo.ino	Wed Aug 27 11:24:40 2014 +0200
@@ -5,6 +5,9 @@
 #include <Servo.h>
 #include <math.h>
 
+// Time of a signle frame, in milliseconds.
+#define FRAME_TIME 10
+
 // Number of servos in legs.
 #define MAX_SERVO 12
 
@@ -153,18 +156,39 @@
 
 void do_accel() {
     /* Report the readings of the acceleration sensor. */
-    int x = 0;
-    int y = 0;
-    int z = 0;
+    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) {
-        x += accel_buffer[i][0];
-        y += accel_buffer[i][1];
-        z += accel_buffer[i][2];
+        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(x / MAX_ACCEL_BUFFER, 0, 676, 0, 255));
-    Serial.write(map(y / MAX_ACCEL_BUFFER, 0, 676, 0, 255));
-    Serial.write(map(z / MAX_ACCEL_BUFFER, 0, 676, 0, 255));
+    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() {
@@ -253,7 +277,7 @@
     unsigned long now = millis();
 
     do_serial_command();
-    if (now > last_tick + 100 || now < last_tick) {
+    if (now > last_tick + FRAME_TIME || now < last_tick) {
         read_accel();
         last_tick = now;
     }