This is a very simple and basic tool that helps in measuring distance [using a rotary encoder ] and also knows the heading reference/orientation via an IMU sensor[MPU-6050].
So that's it.
Further ScopeFor now, our devices only measure 2D space, but we can use them to measure 3D space [the last thing humanity needs to be done is measure 3D space using this tool ]
But some major development that can be added or contributed(π₯²) are :
- Restructuring the control flow in the main.py[software side]
- Changes in many/every function.
- Not using the software in the first place [it was built so that we feel we did something], displaying all the metrics in a separate display[I2C]/the hardware itself.
- Adding a pointer/sight/reference to help the user holding it know where to start and stop.
- Using a better IMU Sensor, we are using MPU6050 on top with DMP, so there is a significant yaw drift[we had only one thing to do π].
- Using a hall effect sensor, instead of a rotary encoder[we just went with the flow, although we knew it was coming].
- Angle interchangeablity.
- Closing the project and not working on stupid ideas, [but it can be efficient if made in a very small form factor
Although it has some limitations, it does work and works fine.
It gives the following output to the user:
- Distance
- Displacement
- Area
- X and Y axis Displacement
Although, it is very easy after we get the two points[start and end]
Demo Video πΊCompensator Testing:CircuitThe First Prototype Consists of :
- Arduino Pro Micro
- Bluetooth HC-05
- Rotary Encoder
- MPU-6050
import bluetooth
import socket
import struct
import sys
from PyQt5.QtWidgets import QApplication, QWidget, QComboBox, QLabel,QFileDialog, QVBoxLayout, QHBoxLayout, QPushButton,QFrame,QDoubleSpinBox
from PyQt5.QtCore import Qt, QThread, pyqtSignal,QPoint, QLineF, QPointF
from PyQt5.QtGui import QPainter, QColor, QPen,QPolygon,QPainterPath
from PyQt5.QtOpenGL import QGLWidget
import OpenGL.GL as gl # python wrapping of OpenGL
from OpenGL import GLU
import math
import ctypes
import json
from OpenGL.arrays import vbo
import numpy as np
x_pos = 0
y_pos = 0
r_value = 0
y_value = 0
rot_const=0
comp_mode=False
x=0.0
y=0.0
z=0.0
class GLWidget(QGLWidget):
def __init__(self, parent=None):
QGLWidget.__init__(self, parent)
def initializeGL(self):
self.qglClearColor(QColor(0, 0, 0)) # initialize the screen to blue
gl.glEnable(gl.GL_DEPTH_TEST) # enable depth testing
self.initGeometry()
self.rotX = 0.0
self.rotY = 0.0
self.rotZ = 0.0
def resizeGL(self, width, height):
gl.glViewport(0, 0, width, height)
gl.glMatrixMode(gl.GL_PROJECTION)
gl.glLoadIdentity()
aspect = width / float(height)
GLU.gluPerspective(45.0, aspect, 1.0, 100.0)
gl.glMatrixMode(gl.GL_MODELVIEW)
def paintGL(self):
gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
gl.glPushMatrix() # push the current matrix to the current stack
gl.glTranslate(0.0, 0.0, -50.0) # third, translate cube to specified depth
gl.glScale(20.0, 20.0, 20.0) # second, scale cube
gl.glRotate(self.rotX, 1.0, 0.0, 0.0)
gl.glRotate(self.rotY, 0.0, 1.0, 0.0)
gl.glRotate(self.rotZ, 0.0, 0.0, 1.0)
gl.glTranslate(-0.5, -0.5, -0.5) # first, translate cube center to origin
gl.glEnableClientState(gl.GL_VERTEX_ARRAY)
gl.glEnableClientState(gl.GL_COLOR_ARRAY)
gl.glVertexPointer(3, gl.GL_FLOAT, 0, self.vertVBO)
gl.glColorPointer(3, gl.GL_FLOAT, 0, self.colorVBO)
gl.glDrawElements(gl.GL_LINES, len(self.cubeIdxArray), gl.GL_UNSIGNED_INT, self.cubeIdxArray)
gl.glDisableClientState(gl.GL_VERTEX_ARRAY)
gl.glDisableClientState(gl.GL_COLOR_ARRAY)
gl.glPopMatrix() # restore the previous modelview matrix
def initGeometry(self):
vertices = [
# Front face
[0.0, 0.0, 0.0],
[1.0, 0.0, 0.0],
[1.0, 1.0, 0.0],
[0.0, 1.0, 0.0],
# Back face
[0.0, 0.0, 1.0],
[1.0, 0.0, 1.0],
[1.0, 1.0, 1.0],
[0.0, 1.0, 1.0],
]
indices = [# Front face
0, 1, 2, 3,
4, 5, 6, 7,
0, 3, 7, 4,
1, 2, 6, 5,
2, 3, 7, 6,
0, 1, 5, 4,]
self.vertVBO = vbo.VBO(np.array(vertices, dtype=np.float32))
self.vertVBO.bind()
self.cubeIdxArray = vbo.VBO(np.array(indices, dtype=np.uint32),
target=gl.GL_ELEMENT_ARRAY_BUFFER)
self.cubeIdxArray.bind()
self.cubeClrArray = np.array(
[[0.0, 0.0, 0.0],
[1.0, 0.0, 0.0],
[1.0, 1.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0],
[1.0, 0.0, 1.0],
[1.0, 1.0, 1.0],
[0.0, 1.0, 1.0 ]])
self.colorVBO = vbo.VBO(np.reshape(self.cubeClrArray,
(1, -1)).astype(np.float32))
self.colorVBO.bind()
def setRot(self, x,y,z):
self.rotX = z
self.rotY = x
self.rotZ = y
self.update()
class BluetoothThread(QThread):
data_received = pyqtSignal(str)
def __init__(self, device_name, drawing_widget,glWidget):
super().__init__()
self.device_name = device_name
self.target_address = None
self.drawing_widget = drawing_widget
self.glWidget =glWidget
self.x_pos_new=x_pos
self.y_pos_new=y_pos
self.rot_en=True
self.rot_en_in=False
self.prev_pitch=0
self.x=0.0
self.y=0.0
self.z=0.0
def run(self):
nearby_devices = bluetooth.discover_devices()
for bdaddr in nearby_devices:
if self.device_name == bluetooth.lookup_name(bdaddr):
self.target_address = bdaddr
break
if self.target_address is not None:
print ("found target bluetooth device with address ", self.target_address)
else:
print ("could not find target bluetooth device nearby")
self.data_received.emit("No Device Found")
return
port = 1
s = socket.socket(socket.AF_BLUETOOTH, socket.SOCK_STREAM, socket.BTPROTO_RFCOMM)
size = 2048
try:
s.connect((self.target_address, port))
except socket.error as e:
print(f"Error connecting to device: {e}")
self.data_received.emit("No Device Found")
return
print("CONNECTED")
d = "0"
s.send(d.encode())
while True:
global x_pos, y_pos,rot_const,x,y,z
self.x,self.y,self.z=x,y,z
data = b''
while len(data) < 8:
data += s.recv(1)
self.x, self.y, self.z, r = struct.unpack('<hhhh', data[:8])
self.z=self.z/100
y_value = self.x / 100
r_value = r / 1
if(self.rot_en_in and r and comp_mode):
r_value= int(r/1+((self.z-self.prev_pitch)/rot_const))
self.rot_en=False
self.data_received.emit(f"x={self.x/100}, y={self.y/100}, z={self.z} ,r={r/1},_r={rot_const}")
self.glWidget.setRot(self.x/100,self.y/100,self.z)
data = data[8:]
self.prev_pitch=self.z
self.x_pos_new = x_pos + r_value * math.cos(math.radians(y_value))
self.y_pos_new = y_pos + r_value * math.sin(math.radians(y_value))
self.drawing_widget.addLine(self.x_pos_new, self.y_pos_new,x_pos,y_pos)
x_pos = self.x_pos_new
y_pos = self.y_pos_new
if(r and self.rot_en and comp_mode):
self.rot_en_in=True
s.close()
class DrawingWidget(QWidget):
def __init__(self, parent=None):
super().__init__(parent)
self.path = QPainterPath()
self.points = []
self.scale_factor = 1.0
def paintEvent(self, event):
qp = QPainter(self)
qp.fillRect(event.rect(), QColor(0, 0, 0))
pen = QPen(QColor(255, 255, 255))
pen.setWidth(2)
qp.setPen(pen)
# Adjust the coordinate system to make the center of the widget the origin
qp.translate(self.width() / 2, self.height() / 2)
qp.scale(self.scale_factor, -self.scale_factor)
# Draw the path using the adjusted coordinate system
qp.drawPath(self.path)
def wheelEvent(self, event):
delta = event.angleDelta().y()
if delta > 0:
self.scale_factor *= 1.1
else:
self.scale_factor /= 1.1
self.update()
def addLine(self, x1, y1, x2, y2):
if x1 != x2 or y1 != y2:
self.path.lineTo(x2, y2)
self.points.append((x2, y2))
self.update()
class MetricsWidget(QWidget):
def __init__(self):
super().__init__(parent=None)
self.scale_factor = 1.0
self.path = QPainterPath()
self.points=[]
self.area = 0.0
self.perimeter=0.0
self.xs=0.0
self.ys=0.0
self.xe=0.0
self.ye=0.0
self.displacement=0.0
self.x_displacement=0.0
self.y_displacement=0.0
self.penBl = QPen(Qt.blue, 2, Qt.SolidLine)
self.penWt = QPen(Qt.white, 2, Qt.SolidLine)
self.penGr = QPen(Qt.green, 2, Qt.SolidLine)
def displayMetrics(self,coordinates):
self.points=coordinates
self.xs,self.ys=map(int, self.points[1])
self.xe,self.ye=map(int, self.points[-1])
self.calculate_area()
self.perimeter = sum(QLineF(QPointF(*p1), QPointF(*p2)).length() for p1, p2 in zip(self.points, self.points[1:]))
self.displacement = math.hypot(self.xe - self.xs, self.ye - self.ys)
self.x_displacement = self.xe - self.xs
self.y_displacement = self.ye - self.ys
# result_label = QLabel(f"Area: {self.area:.2f}\nPerimeter: {self.perimeter:.2f}")
# print(f"Area: {self.area:.2f}\nPerimeter: {self.perimeter:.2f}\n")
self.update()
def paintEvent(self,e):
qp = QPainter(self)
qp.fillRect(e.rect(), QColor(0, 0, 0))
qp.translate(self.width() / 2, self.height() / 2)
qp.scale(1, -1)
qp.scale(self.scale_factor, -self.scale_factor)
if((self.xs and self.xe and self.ys and self.ye)!=None):
self.draw_lines(qp)
self.draw_polygon(qp)
self.drawPoint(qp)
self.drawDisplacement(qp)
def wheelEvent(self, event):
delta = event.angleDelta().y()
if delta > 0:
self.scale_factor *= 1.1
else:
self.scale_factor /= 1.1
self.update()
def drawPoint(self,qp):
qp.setPen(QPen(Qt.red, 8))
if len(self.points) >= 1:
qp.drawPoint(self.xs, self.ys)
qp.setPen(QPen(Qt.blue, 8))
qp.drawPoint(self.xe, self.ye)
def drawDisplacement(self,qp):
qp.setPen(self.penGr)
qp.drawLine(int(self.xs), int(self.ys), int(self.xe), int(self.ye))
qp.setPen(self.penWt)
qp.drawLine(int(self.xs), int(self.ys), int(self.xs + self.x_displacement), int(self.ys))
qp.setPen(self.penBl)
qp.drawLine(int(self.xs), int(self.ys), int(self.xs), int(self.ys + self.y_displacement))
def draw_lines(self, qp):
pen = QPen(Qt.black, 2, Qt.SolidLine)
qp.setPen(pen)
for i in range(len(self.points) - 1):
x1, y1 = map(int, self.points[i])
x2, y2 = map(int, self.points[i+1])
qp.drawLine(int(x1), int(y1), int(x2), int(y2))
def draw_polygon(self, qp):
polygon = QPolygon([QPoint(int(p[0]), int(p[1])) for p in self.points])
brush = Qt.yellow
qp.setBrush(brush)
qp.drawPolygon(polygon)
def calculate_area(self):
n = len(self.points)
for i in range(n):
j = (i + 1) % n
self.area += self.points[i][0] * self.points[j][1] - self.points[j][0] * self.points[i][1]
self.area=abs(self.area)/2
class MainWindow(QWidget):
def __init__(self):
super().__init__()
self.setWindowTitle("Accelerometer Data Visualization")
self.setGeometry(100, 100, 1000, 3000)
self.feature_collection = {"type": "FeatureCollection", "features": []}
global rot_const
# Create a combo box to select the Bluetooth device to connect to
self.device_combo = QComboBox()
self.device_combo.setMaximumWidth(300)
self.device_combo.addItems([bluetooth.lookup_name(address) for address in bluetooth.discover_devices()])
self.connect_button = QPushButton("Connect")
self.connect_button.clicked.connect(self.connect_clicked)
# Create a label to display the selected Bluetooth device
self.data_recv_label = QLabel()
self.metric_label = QLabel()
# Create a frame for the label
self.data_recv_frame = QFrame()
self.data_recv_frame.setFrameShape(QFrame.WinPanel)
self.data_recv_frame.setFrameShadow(QFrame.Sunken)
self.data_recv_frame.setAutoFillBackground(True)
self.reset_button = QPushButton("Reset")
self.reset_button.clicked.connect(self.reset_clicked)
self.generate_button = QPushButton("Generate")
self.generate_button.clicked.connect(self.generate_button_clicked)
self.geojson_button = QPushButton("Save-GeoJSON")
self.geojson_button.clicked.connect(self.geoJsonSave)
self.geojson_button.setEnabled(False)
self.viewjson_button = QPushButton("View-GeoJSON")
self.viewjson_button.clicked.connect(self.viewGeoJSON)
# Create a QDoubleSpinBox widget with a default value of 0.0
self.length_const = QDoubleSpinBox()
self.length_const.setRange(-10.0, 10.0)
self.length_const.setSingleStep(0.01)
self.length_const.setValue(0.17)
self.rotation_const = QDoubleSpinBox()
self.rotation_const.setRange(-360.0,360.0)
self.rotation_const.setSingleStep(0.5);
self.rotation_const.setValue(30)
rot_const=360/self.rotation_const.value()
constant_layout = QHBoxLayout()
constant_layout.addWidget(QLabel("Length Const:"))
constant_layout.addWidget(self.length_const)
rotation_const_laout = QHBoxLayout()
rotation_const_laout.addWidget(QLabel("Steps/Rotation:"))
rotation_const_laout.addWidget(self.rotation_const)
self.compensator_Switch = QPushButton('Compensator', self)
self.compensator_Switch.setCheckable(True)
self.compensator_Switch.move(110, 20)
self.compensator_Switch.clicked[bool].connect(self.compensatorMode)
settings_layout =QVBoxLayout()
settings_layout.addWidget(self.compensator_Switch)
settings_layout.addLayout(constant_layout)
settings_layout.addLayout(rotation_const_laout)
# Button UI
self.metric_plot = MetricsWidget()
self.metric_plot.setVisible(True)
# Add the label to the frame
frame_layout = QVBoxLayout()
frame_layout.addWidget(self.data_recv_label)
frame_layout.addWidget(self.metric_label)
self.data_recv_frame.setLayout(frame_layout)
# Create a layout for plotter
self.paint_plot = DrawingWidget()
self.paint_plot.setVisible(True)
plotLayout = QVBoxLayout()
plotLayout.addWidget(self.paint_plot)
plotLayout.addWidget(self.metric_plot)
#Glwidget
self.glWidget = GLWidget()
buttons_layout = QVBoxLayout()
buttons_layout.addWidget(self.glWidget)
buttons_layout.addWidget(QLabel("Select Bluetooth Device: "))
buttons_layout.addWidget(self.device_combo)
buttons_layout.addWidget(self.connect_button)
# Add the QDoubleSpinBox widget and label to the layout
buttons_layout.addLayout(settings_layout)
buttons_layout.addWidget(self.generate_button)
buttons_layout.addWidget(self.reset_button)
buttons_layout.addWidget(self.geojson_button)
buttons_layout.addWidget(self.viewjson_button)
buttons_layout.addWidget(self.data_recv_frame)
# Create a layout for the UI
layout = QHBoxLayout()
layout.addLayout(plotLayout, 3)
layout.addLayout(buttons_layout, 2)
self.setLayout(layout)
self.thread = None
def viewGeoJSON(self):
vpoints=[]
self.feature_collection = {"type": "FeatureCollection", "features": []}
# prompt the user to select a file to load
options = QFileDialog.Options()
options |= QFileDialog.DontUseNativeDialog
filename, _ = QFileDialog.getOpenFileName(self, "Open file", "", "GeoJSON Files (*.geojson)", options=options)
if filename:
with open(filename) as f:
data = json.load(f)
for feature in data['features']:
if feature['geometry']['type'] == 'Point':
x, y = feature['geometry']['coordinates']
vpoints.append((x,y))
self.metric_plot.displayMetrics(vpoints)
self.metric_label.setText(f"Area:{self.metric_plot.area*self.length_const.value():.2f}\nDistance:{self.metric_plot.perimeter*self.length_const.value():.2f}\nDisplacement:{self.metric_plot.displacement*self.length_const.value():.2f}\nPerimeter:{self.metric_plot.perimeter*self.length_const.value()}\nX-Displacement:{self.metric_plot.x_displacement*self.length_const.value()}\nY-Displacement:{self.metric_plot.y_displacement*self.length_const.value()}\n")
def geoJsonSave(self):
# create a GeoJSON feature collection object
if 'features' not in self.feature_collection:
self.feature_collection['features'] = []
for point in self.metric_plot.points:
x,y = point
self.feature_collection['features'].append({"type": "Feature", "geometry": {"type": "Point", "coordinates": [x, y]}})
# prompt the user to choose a filename and location
options = QFileDialog.Options()
options |= QFileDialog.DontUseNativeDialog
filename, _ = QFileDialog.getSaveFileName(self, "Save file", "", "GeoJSON Files (*.geojson)", options=options)
if filename:
with open(filename, "w") as outfile:
json.dump(self.feature_collection, outfile)
def compensatorMode(self,pressed):
global comp_mode
if pressed:
comp_mode=True
else:
comp_mode=False
def generate_button_clicked(self):
coordinates=self.paint_plot.points
self.metric_plot.displayMetrics(coordinates)
self.geojson_button.setEnabled(True)
def connect_clicked(self):
device_name = self.device_combo.currentText()
if self.thread is not None:
self.thread.terminate()
self.thread = BluetoothThread(device_name,self.paint_plot,self.glWidget)
self.thread.data_received.connect(self.data_received_text)
self.thread.finished.connect(self.connect_finished)
self.connect_button.setEnabled(False)
self.thread.start()
def data_received_text(self, data):
self.data_recv_label.setText(data)
self.metric_label.setText(f"Area:{self.metric_plot.area*self.length_const.value():.2f}\nDistance:{self.metric_plot.perimeter*self.length_const.value():.2f}\nDisplacement:{self.metric_plot.displacement*self.length_const.value():.2f}\nPerimeter:{self.metric_plot.perimeter*self.length_const.value()}\nX-Displacement:{self.metric_plot.x_displacement*self.length_const.value()}\nY-Displacement:{self.metric_plot.y_displacement*self.length_const.value()}\n")
def connect_finished(self):
self.connect_button.setEnabled(True)
def reset_clicked(self):
self.geojson_button.setEnabled(False)
self.paint_plot.path = QPainterPath()
self.metric_plot.path=QPainterPath()
self.paint_plot.update()
self.metric_plot.update()
global x_pos, y_pos,rot_const
x_pos = 0
y_pos = 0
rot_const=360/self.rotation_const.value()
self.paint_plot.points.clear()
self.metric_plot.points.clear()
self.metric_plot.area=0.0
self.metric_plot.perimeter=0.0
self.metric_plot.xs=0.0
self.metric_plot.xe=0.0
self.metric_plot.ys=0.0
self.metric_plot.ye=0.0
self.metric_plot.x_displacement=self.metric_plot.y_displacement=0.0
self.metric_plot.displacement=0.0
self.feature_collection.clear()
self.metric_label.clear()
def handle_text_changed(self, text):
self.textbox.insertPlainText(text)
self.textbox.insertPlainText("\n")
self.keyboard.type(text)
if __name__ == '__main__':
app = QApplication(sys.argv)
window = MainWindow()
window.show()
sys.exit(app.exec_())
Arduino Code :#include <ArduinoJson.h>
#include "I2Cdev.h"
#include <SoftwareSerial.h>
#include "MPU6050_6Axis_MotionApps20.h"
#include <Wire.h>
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
//#define OUTPUT_READABLE_YAWPITCHROLL_SERIAL
#define OUTPUT_READABLE_YAWPITCHROLL_SERIAL1
char buff[100];
#define BAUD_RATE 9600
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
const int encoderPinA = 4;
const int encoderPinB = 5;
int previousEncoderState = 0;
int currentEncoderState = 0;
int encoderPosition = 0,temp=0;
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
pinMode(encoderPinA, INPUT);
pinMode(encoderPinB, INPUT);
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(BAUD_RATE);
#ifdef OUTPUT_READABLE_YAWPITCHROLL_SERIAL1
Serial1.begin(BAUD_RATE);
#endif
// while (!Serial);
// // initialize device
// Serial.println(F("Initializing I2C devices..."));
// mpu.initialize();
// pinMode(INTERRUPT_PIN, INPUT);
// // verify connection
// Serial.println(F("Testing device connections..."));
// Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// // wait for ready
// Serial.println(F("\nSend any character to begin DMP programming and demo: "));
// while (Serial.available() && Serial.read()); // empty buffer
// while (!Serial.available()); // wait for data
// while (Serial.available() && Serial.read()); // empty buffer again
while (!Serial1);
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
if(Serial1.available())
Serial.println(mpu.testConnection() ? F("P") : F("F"));
while (Serial1.available() && Serial1.read()); // empty buffer
while (!Serial1.available()); // wait for data
while (Serial1.available() && Serial1.read()); // empty buffer again
// load and configure the DMP
//Serial1.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
}
void rotEnc() {
// Read the state of the rotary encoder
previousEncoderState = currentEncoderState;
currentEncoderState = digitalRead(encoderPinA) << 1 | digitalRead(encoderPinB);
// Check for rotary encoder movement
if (previousEncoderState != currentEncoderState)
{
if ((previousEncoderState == 0b00 && currentEncoderState == 0b01) ||(previousEncoderState == 0b01 && currentEncoderState == 0b11) ||
(previousEncoderState == 0b11 && currentEncoderState == 0b10) || (previousEncoderState == 0b10 && currentEncoderState == 0b00))
{
encoderPosition++;
}
else
{
encoderPosition--;
}
}
}
void loop() {
rotEnc();
// if programming failed, don't try to do anything
if (!dmpReady) return;
// read a packet from FIFO
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
#ifdef OUTPUT_READABLE_YAWPITCHROLL_SERIAL
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print(ypr[0] * 180/M_PI);
Serial.print(",");
Serial.print(ypr[1] * 180/M_PI);
Serial.print(",");
Serial.print(ypr[2] * 180/M_PI);
Serial.print(",");
Serial.println(encoderPosition);
#endif
#ifdef OUTPUT_READABLE_YAWPITCHROLL_SERIAL1
// display Euler angles in degrees
// Serial1.println("HELLO/n");
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
temp=encoderPosition-temp;
int16_t x=ypr[0]*180/M_PI * 100;
int16_t y=ypr[1]*180/M_PI * 100;
int16_t z=ypr[2]*180/M_PI * 100;
int16_t r = (int16_t) temp;
byte buf[8];
buf[0] = x & 255;
buf[1] = (x >> 8) & 255;
buf[2] = y & 255;
buf[3] = (y >> 8) & 255;
buf[4] = z & 255;
buf[5] = (z >> 8) & 255;
buf[6] = r & 255;
buf[7] = (r >> 8) & 255;
Serial1.write(buf,sizeof(buf));
temp=encoderPosition;
// Serial1.print(ypr[0]*180/M_PI);
// Serial1.print(",");
// Serial1.print(ypr[1]*180/M_PI);
// Serial1.print(",");
// Serial1.println(ypr[2]*180/M_PI);
#endif
}
}
Comments