I am trying to achieve something like
Arduino with Gyroscope(SEND AXIS) → Bluetooth HC-06 → (SEND AXIS) → AndroidOS( Receive axis ) → Godot deployed on android read bluetooth data paired with …
This is the source code =
ARDUINO
// (c) Michael Schoeffler 2017, http://www.mschoeffler.de
// SDAA5 SCLA4 in old sketch
#include "Wire.h" // This library allows you to communicate with I2C devices.
const int MPU_ADDR = 0x68; // I2C address of the MPU-6050. If AD0 pin is set to HIGH, the I2C address will be 0x69.
int16_t accelerometer_x, accelerometer_y, accelerometer_z; // variables for accelerometer raw data
int16_t gyro_x, gyro_y, gyro_z; // variables for gyro raw data
int16_t temperature; // variables for temperature data
char tmp_str[7]; // temporary variable used in convert function
char* convert_int16_to_str(int16_t i) { // converts int16 to string. Moreover, resulting strings will have the same length in the debug monitor.
sprintf(tmp_str, "%6d", i);
return tmp_str;
}
void setup() {
Serial.begin(9600);
Wire.begin();
Wire.beginTransmission(MPU_ADDR); // Begins a transmission to the I2C slave (GY-521 board)
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
}
void loop() {
Wire.beginTransmission(MPU_ADDR);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) [MPU-6000 and MPU-6050 Register Map and Descriptions Revision 4.2, p.40]
Wire.endTransmission(false); // the parameter indicates that the Arduino will send a restart. As a result, the connection is kept active.
Wire.requestFrom(MPU_ADDR, 7*2, true); // request a total of 7*2=14 registers
// "Wire.read()<<8 | Wire.read();" means two registers are read and stored in the same variable
accelerometer_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x3B (ACCEL_XOUT_H) and 0x3C (ACCEL_XOUT_L)
accelerometer_y = Wire.read()<<8 | Wire.read(); // reading registers: 0x3D (ACCEL_YOUT_H) and 0x3E (ACCEL_YOUT_L)
accelerometer_z = Wire.read()<<8 | Wire.read(); // reading registers: 0x3F (ACCEL_ZOUT_H) and 0x40 (ACCEL_ZOUT_L)
temperature = Wire.read()<<8 | Wire.read(); // reading registers: 0x41 (TEMP_OUT_H) and 0x42 (TEMP_OUT_L)
gyro_x = Wire.read()<<8 | Wire.read(); // reading registers: 0x43 (GYRO_XOUT_H) and 0x44 (GYRO_XOUT_L)
gyro_y = Wire.read()<<8 | Wire.read(); // reading registers: 0x45 (GYRO_YOUT_H) and 0x46 (GYRO_YOUT_L)
gyro_z = Wire.read()<<8 | Wire.read(); // reading registers: 0x47 (GYRO_ZOUT_H) and 0x48 (GYRO_ZOUT_L)
// print out data
Serial.print("aX = "); Serial.print(convert_int16_to_str(accelerometer_x));
Serial.print(" | aY = "); Serial.print(convert_int16_to_str(accelerometer_y));
Serial.print(" | aZ = "); Serial.print(convert_int16_to_str(accelerometer_z));
// the following equation was taken from the documentation [MPU-6000/MPU-6050 Register Map and Description, p.30]
Serial.print(" | tmp = "); Serial.print(temperature/340.00+36.53);
Serial.print(" | gX = "); Serial.print(convert_int16_to_str(gyro_x));
Serial.print(" | gY = "); Serial.print(convert_int16_to_str(gyro_y));
Serial.print(" | gZ = "); Serial.print(convert_int16_to_str(gyro_z));
Serial.println();
// delay
delay(100);
}
Godot 4.3
extends Node
var http = HTTPRequest.new()
var bluetooth_adapter = null
var input_stream = null
var is_receiving = false
var DebugLabel = null
func _ready():
DebugLabel = $Control/DebugText
if OS.get_name() != "Android":
# Write data to a file
var file = FileAccess.open("/home/tigret/Desktop/Projects/Godot/Godot43/OTHERSOFTWARENECESSARY/dataMEOW.txt", FileAccess.WRITE)
if file:
file.store_string("Hello from Godot!")
file.close()
else:
print("Failed to open file for writing")
# Read data from a file
file = FileAccess.open("/home/tigret/Desktop/Projects/Godot/Godot43/OTHERSOFTWARENECESSARY/data.txt", FileAccess.READ)
if file:
var content = file.get_as_text()
file.close()
print("Read from file: ", content)
else:
print("Failed to open file for reading")
DebugLabel.text = "Meow"
if OS.get_name() == "Android":
bluetooth_adapter = JavaClassWrapper.wrap("android.bluetooth.BluetoothAdapter")
var adapter = bluetooth_adapter.getDefaultAdapter()
if adapter != null:
if adapter.isEnabled():
DebugLabel.text = "Bluetooth està activat"
print("Bluetooth està activat")
else:
DebugLabel.text = "Bluetooth no està activat"
print("Bluetooth no està activat")
else:
DebugLabel.text = "Dispositiu no compatible amb Bluetooth"
print("Dispositiu no compatible amb Bluetooth")
else:
DebugLabel.text = "Bluetooth només està suportat a Android"
print("Bluetooth només està suportat a Android")
func start_bluetooth():
if OS.get_name() == "Android" and bluetooth_adapter != null:
var adapter = bluetooth_adapter.getDefaultAdapter()
if not adapter.isEnabled():
adapter.enable()
print("Bluetooth activat")
else:
print("Bluetooth ja estava activat")
func stop_bluetooth():
if OS.get_name() == "Android" and bluetooth_adapter != null:
var adapter = bluetooth_adapter.getDefaultAdapter()
if adapter.isEnabled():
adapter.disable()
print("Bluetooth desactivat")
else:
print("Bluetooth ja estava desactivat")
func start_receiving_data():
if OS.get_name() == "Android" and input_stream != null:
is_receiving = true
while is_receiving:
var buffer = input_stream.read() # Llegeix les dades del fluxe
if buffer.size() > 0:
var data = buffer.get_string_from_utf8() # Converteix les dades a una cadena
print("Dades rebudes: " + data)
# Aquí pots processar les dades rebudes (per exemple, mostrar-les a la pantalla)
else:
break # Si no hi ha dades, surt del bucle
else:
print("No s'ha pogut començar a rebre dades")
func _on_request_completed(result, response_code, headers, body):
var response = body.get_string_from_utf8()
print("Received from server: ", response)
# Below are a number of helper functions that show how you can use the raw sensor data to determine the orientation
# of your phone/device. The cheapest phones only have an accelerometer only the most expensive phones have all three.
# Note that none of this logic filters data. Filters introduce lag but also provide stability. There are plenty
# of examples on the internet on how to implement these. I wanted to keep this straight forward.
# We draw a few arrow objects to visualize the vectors and two cubes to show two implementation for orientating
# these cubes to our phones orientation.
# This is a 3D example however reading the phones orientation is also invaluable for 2D
# This function calculates a rotation matrix based on a direction vector. As our arrows are cylindrical we don't
# care about the rotation around this axis.
func get_basis_for_arrow(p_vector):
var rotate = Basis()
# as our arrow points up, Y = our direction vector
rotate.y = p_vector.normalized()
# get an arbitrary vector we can use to calculate our other two vectors
var v = Vector3(1.0, 0.0, 0.0)
if abs(v.dot(rotate.y)) > 0.9:
v = Vector3(0.0, 1.0, 0.0)
# use our vector to get a vector perpendicular to our two vectors
rotate.x = rotate.y.cross(v).normalized()
# and the cross product again gives us our final vector perpendicular to our previous two vectors
rotate.z = rotate.x.cross(rotate.y).normalized()
return rotate
# This function combines the magnetometer reading with the gravity vector to get a vector that points due north
func calc_north(p_grav, p_mag):
# Always use normalized vectors!
p_grav = p_grav.normalized()
# Calculate east (or is it west) by getting our cross product.
# The cross product of two normalized vectors returns a vector that
# is perpendicular to our two vectors
var east = p_grav.cross(p_mag.normalized()).normalized()
# Cross again to get our horizon aligned north
return east.cross(p_grav).normalized()
# This function creates an orientation matrix using the magnetometer and gravity vector as inputs.
func orientate_by_mag_and_grav(p_mag, p_grav):
var rotate = Basis()
# as always, normalize!
p_mag = p_mag.normalized()
# gravity points down, so - gravity points up!
rotate.y = -p_grav.normalized()
# Cross products with our magnetic north gives an aligned east (or west, I always forget)
rotate.x = rotate.y.cross(p_mag)
# And cross product again and we get our aligned north completing our matrix
rotate.z = rotate.x.cross(rotate.y)
return rotate
# This function takes our gyro input and update an orientation matrix accordingly
# The gyro is special as this vector does not contain a direction but rather a
# rotational velocity. This is why we multiply our values with delta.
func rotate_by_gyro(p_gyro, p_basis, p_delta):
var rotate = Basis()
rotate = rotate.rotated(Vector3(5.0,7.0,125.0), -p_gyro.x * p_delta)
rotate = rotate.rotated(p_basis.y, -p_gyro.y * p_delta)
rotate = rotate.rotated(p_basis.z, -p_gyro.z * p_delta)
return rotate * p_basis
# This function corrects the drift in our matrix by our gravity vector
func drift_correction(p_basis, p_grav):
# as always, make sure our vector is normalized but also invert as our gravity points down
var real_up = -p_grav.normalized()
# start by calculating the dot product, this gives us the cosine angle between our two vectors
var dot = p_basis.y.dot(real_up)
# if our dot is 1.0 we're good
if dot < 1.0:
# the cross between our two vectors gives us a vector perpendicular to our two vectors
var axis = p_basis.y.cross(real_up).normalized()
var correction = Basis(axis, acos(dot))
p_basis = correction * p_basis
return p_basis
func DestriaLaString(StringQueDestriar):
var StrLen = StringQueDestriar.length()
var FinalString = ""
for i in range(StrLen):
if StringQueDestriar[i] == 'g' && StringQueDestriar[i+1] == 'Y':
for i2 in range(8):
if(StringQueDestriar[i+i2+6] == "|"):
return FinalString
else:
if( StringQueDestriar[i+i2+6] != "" && StringQueDestriar[i+i2+6] != "|"):
FinalString = FinalString + StringQueDestriar[i+i2+6]
return FinalString
return ""
return
func _process(delta):
# Read data from a file
if OS.get_name() != "Android":
var file = FileAccess.open("/home/tigret/Desktop/Projects/Godot/Godot43/OTHERSOFTWARENECESSARY/data.txt", FileAccess.READ)
if file:
var content = file.get_as_text()
#print( DestriaLaString(file.get_as_text()) )
var gyro_and_grav = $Arrows
var box = get_node("Box")
#print( file.get_as_text() )
var CachedString = content
if ( DestriaLaString(CachedString).length() ) > 2:
#box.set_rotation_degrees( Vector3( DestriaLaString(CachedString).to_float() /10, 0.0, 0.0) )
#print( box.get_rotation() );
if abs( DestriaLaString(CachedString).to_float() ) > 1000.0:
#box.transform.basis.rotated(Vector3(100.0, 10.0, 10.0), DestriaLaString(CachedString).to_float() /1.0)
box.rotate_object_local(Vector3.RIGHT, DestriaLaString(CachedString).to_float() /100000.0)
#set_rotation_degrees( box.get_rotation() + Vector3( DestriaLaString(CachedString).to_float() /500.0, 0.0, 0.0) )
#gyro_and_grav.transform.basis.x.x = DestriaLaString(file.get_as_text()).to_float()/10000
#gyro_and_grav.transform.basis.x.z += 0.25
file.close()
else:
print("Failed to open file for reading")
#if socket.get_status() == 1:
# print("Received from server: TIER 1 should receive stuff from server" )
# var bytes_available = socket.get_available_bytes()
# var TheSocketString = socket.get_utf8_string(socket.get_available_bytes())
return
# Get our data
var acc = Input.get_accelerometer()
var grav = Input.get_gravity()
var mag = Input.get_magnetometer()
var gyro = Input.get_gyroscope()
# Show our base values
var format = "%.05f"
%AccX.text = format % acc.x
%AccY.text = format % acc.y
%AccZ.text = format % acc.z
%GravX.text = format % grav.x
%GravY.text = format % grav.y
%GravZ.text = format % grav.z
%MagX.text = format % mag.x
%MagY.text = format % mag.y
%MagZ.text = format % mag.z
%GyroX.text = format % gyro.x
%GyroY.text = format % gyro.y
%GyroZ.text = format % gyro.z
# Check if we have all needed data
if grav.length() < 0.1:
if acc.length() < 0.1:
# we don't have either...
grav = Vector3(0.0, -1.0, 0.0)
else:
# The gravity vector is calculated by the OS by combining the other sensor inputs.
# If we don't have a gravity vector, from now on, use accelerometer...
grav = acc
if mag.length() < 0.1:
mag = Vector3(1.0, 0.0, 0.0)
# Update our arrow showing gravity
$Arrows/AccelerometerArrow.transform.basis = get_basis_for_arrow(grav)
# Update our arrow showing our magnetometer
# Note that in absence of other strong magnetic forces this will point to magnetic north, which is not horizontal thanks to the earth being, uhm, round
$Arrows/MagnetoArrow.transform.basis = get_basis_for_arrow(mag)
# Calculate our north vector and show that
var north = calc_north(grav, mag)
$Arrows/NorthArrow.transform.basis = get_basis_for_arrow(north)
# Combine our magnetometer and gravity vector to position our box. This will be fairly accurate
# but our magnetometer can be easily influenced by magnets. Cheaper phones often don't have gyros
# so it is a good backup.
var mag_and_grav = $Boxes/MagAndGrav
mag_and_grav.transform.basis = orientate_by_mag_and_grav(mag, grav).orthonormalized()
# Using our gyro and do a drift correction using our gravity vector gives the best result
var gyro_and_grav = $Boxes/GyroAndGrav
var new_basis = rotate_by_gyro(gyro, gyro_and_grav.transform.basis, delta).orthonormalized()
gyro_and_grav.transform.basis = drift_correction(new_basis, grav)
I can read the info from arduino to PC but not from Arduino to Cell Phone, why does it fail? I check the permissions check box and it all fails…
It throws an error that invalid library the “” bluetooth_adapter = JavaClassWrapper.wrap(“android.bluetooth.BluetoothAdapter”) because of this line…