Mercurial > ukubik
changeset 80:3f210f7632c6
Use MPU-6050 for accelerometer
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Sat, 11 Oct 2014 14:55:47 +0200 |
| parents | c7712053e73e |
| children | 15b840b5b756 |
| files | robot.py servo.py trot.py ukubik/accel.ino ukubik/ukubik.ino |
| diffstat | 5 files changed, 56 insertions(+), 52 deletions(-) [+] |
line wrap: on
line diff
--- a/robot.py Wed Oct 01 17:50:29 2014 +0200 +++ b/robot.py Sat Oct 11 14:55:47 2014 +0200 @@ -59,9 +59,9 @@ def get_accel(self): 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 + sensors[0:3], # accel + sensors[3:6], # gyro + sensors[6], # temperature ] def get_battery(self):
--- a/servo.py Wed Oct 01 17:50:29 2014 +0200 +++ b/servo.py Sat Oct 11 14:55:47 2014 +0200 @@ -1,6 +1,8 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- +import struct + def _normalize_degrees(angle): """Convert an angle to range +-180 degree range.""" @@ -14,6 +16,9 @@ value = int(value) return chr(value & 0xff) + (chr((value & 0xff00) >> 8)) +def _read_word(serial): + return struct.unpack('>h', serial.read(2))[0] + class Servo(object): span = 180 center = 90 @@ -45,7 +50,7 @@ self.serial.write(command) while self.serial.read(1) != '\xff': pass - return [ord(c) for c in self.serial.read(9)] + return [_read_word(self.serial) for w in range(7)] def beep(self, frequency, duration): command = '\xf1'
--- a/trot.py Wed Oct 01 17:50:29 2014 +0200 +++ b/trot.py Sat Oct 11 14:55:47 2014 +0200 @@ -32,7 +32,7 @@ def _lower_leg(self, leg): """Puts the leg on the ground.""" leg.move_to((None, None, leg.home[2] + 1)) - for frame in range(2): + for frame in range(1): yield leg.move_to((None, None, leg.home[2])) self.on_ground.add(leg) @@ -52,7 +52,7 @@ None, )) leg.rotate_by(self.rotation * leg.x_dir * leg.y_dir * 10) - for frame in range(4): + for frame in range(3): yield def stomp_anim(self): @@ -73,7 +73,7 @@ for frame, frame2 in zip(self._lower_leg(leg), self._lower_leg(leg2)): yield - for frame in range(10): + for frame in range(14): yield def retract_anim(self): @@ -94,7 +94,7 @@ import serial import kubik - FPS = 50 + FPS = 40 robot = kubik.Robot(serial.Serial('/dev/rfcomm0', 115200)) creep = Creep(robot)
--- a/ukubik/accel.ino Wed Oct 01 17:50:29 2014 +0200 +++ b/ukubik/accel.ino Sat Oct 11 14:55:47 2014 +0200 @@ -1,53 +1,51 @@ -// 3.3V maximum for the acceleration sensor. -#define ACCEL_MAX 676 +#include <Wire.h> -// How many acceleration measurements to cache. -#define MAX_ACCEL_BUFFER 16 - - -int accel_buffer[MAX_ACCEL_BUFFER][3]; +// I2C address of the MPU-6050 +const int MPU = 0x68; 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 accel_setup() { + Wire.begin(); + Wire.beginTransmission(MPU); + Wire.write(0x6B); // PWR_MGMT_1 register + Wire.write(0); // set to zero (wakes up the MPU-6050) + Wire.endTransmission(true); +} + +int16_t _wire_read_int16() { + return Wire.read() << 8 | Wire.read(); +} + +void _serial_write_int16(int16_t what) { + Serial.write((what >> 8) & 0xff); + Serial.write(what & 0xff); } 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; + int16_t acX, acY, acZ, tmp, gyX, gyY, gyZ; + + Wire.beginTransmission(MPU); + Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) + Wire.endTransmission(false); + Wire.requestFrom(MPU, 14, true); // request a total of 14 registers + acX = _wire_read_int16(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) + acY = _wire_read_int16(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) + acZ = _wire_read_int16(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) + tmp = _wire_read_int16(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) + gyX = _wire_read_int16(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) + gyY = _wire_read_int16(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) + gyZ = _wire_read_int16(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) + tmp = tmp / 340.00 + 36.53; // Convert to degrees Celsius. + 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)); + _serial_write_int16(acX); + _serial_write_int16(acY); + _serial_write_int16(acZ); + _serial_write_int16(gyX); + _serial_write_int16(gyY); + _serial_write_int16(gyZ); + _serial_write_int16(tmp); }