-
Notifications
You must be signed in to change notification settings - Fork 17
State class #20
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
base: main
Are you sure you want to change the base?
State class #20
Changes from all commits
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,12 +1,10 @@ | ||
| #ifndef QBEAD_H | ||
| #define QBEAD_H | ||
|
|
||
|
|
||
| #include <Arduino.h> | ||
| #include <Adafruit_NeoPixel.h> | ||
| #include <LSM6DS3.h> | ||
| #include <math.h> | ||
|
|
||
| #include <bluefruit.h> | ||
|
|
||
| // default configs | ||
|
|
@@ -91,8 +89,7 @@ float theta(float x, float y, float z) { | |
| return theta; | ||
| } | ||
|
|
||
| void connect_callback(uint16_t conn_handle) | ||
| { | ||
| void connect_callback(uint16_t conn_handle){ | ||
| // Get the reference to current connection | ||
| BLEConnection* connection = Bluefruit.Connection(conn_handle); | ||
|
|
||
|
|
@@ -103,6 +100,111 @@ void connect_callback(uint16_t conn_handle) | |
| Serial.println(central_name); | ||
| } | ||
|
|
||
| // State Class Definition | ||
| class State { | ||
| private: | ||
| float x, y, z; // Cartesian coordinates | ||
| float theta, phi; // Spherical coordinates | ||
|
|
||
| void cartesianToSpherical() { | ||
| float l = sqrt(x * x + y * y + z * z); | ||
| if (l == 0) l = 1; | ||
| theta = acos(z / l) * 180 / PI; | ||
| phi = atan2(y, x) * 180 / PI; | ||
| if (phi < 0) phi += 360; | ||
| } | ||
|
|
||
| void sphericalToCartesian() { | ||
| float r = 1; | ||
| x = r * sin(theta * PI / 180.0) * cos(phi * PI / 180.0); | ||
| y = r * sin(theta * PI / 180.0) * sin(phi * PI / 180.0); | ||
| z = r * cos(theta * PI / 180.0); | ||
| } | ||
|
|
||
| public: | ||
| // Constructors | ||
| State() : x(0), y(0), z(1) { | ||
| cartesianToSpherical(); | ||
| } | ||
|
|
||
| State(float x_init, float y_init, float z_init) : x(x_init), y(y_init), z(z_init) { | ||
| cartesianToSpherical(); | ||
| } | ||
|
|
||
| State(float theta_init, float phi_init) : theta(theta_init), phi(phi_init) { | ||
| sphericalToCartesian(); | ||
| } | ||
|
|
||
| // Setters and getters | ||
| void setX(float new_x) { | ||
| x = new_x; | ||
| cartesianToSpherical(); | ||
| } | ||
|
|
||
| void setY(float new_y) { | ||
| y = new_y; | ||
| cartesianToSpherical(); | ||
| } | ||
|
|
||
| void setZ(float new_z) { | ||
| z = new_z; | ||
| cartesianToSpherical(); | ||
| } | ||
|
|
||
| void setXYZ(float new_x, float new_y, float new_z) { | ||
| x = new_x; | ||
| y = new_y; | ||
| z = new_z; | ||
| cartesianToSpherical(); | ||
| } | ||
|
|
||
| void setTheta(float new_theta) { | ||
| theta = new_theta; | ||
| sphericalToCartesian(); | ||
| } | ||
|
|
||
| void setPhi(float new_phi) { | ||
| phi = new_phi; | ||
| sphericalToCartesian(); | ||
| } | ||
|
|
||
| void setThetaPhi(float new_theta, float new_phi) { | ||
| theta = new_theta; | ||
| phi = new_phi; | ||
| sphericalToCartesian(); | ||
| } | ||
|
|
||
| float getX() const { | ||
| return x; | ||
| } | ||
|
|
||
| float getY() const { | ||
| return y; | ||
| } | ||
|
|
||
| float getZ() const { | ||
| return z; | ||
| } | ||
|
|
||
| float getTheta() const { | ||
| return theta; | ||
| } | ||
|
|
||
| float getPhi() const { | ||
| return phi; | ||
| } | ||
|
|
||
| // Method to print the state | ||
| void printState() { | ||
| Serial.print("Cartesian: x = "); Serial.print(x); | ||
| Serial.print(", y = "); Serial.print(y); | ||
| Serial.print(", z = "); Serial.println(z); | ||
|
|
||
| Serial.print("Spherical: theta = "); Serial.print(theta); | ||
| Serial.print(", phi = "); Serial.println(phi); | ||
| } | ||
| }; | ||
|
|
||
| namespace Qbead { | ||
|
|
||
| class Qbead { | ||
|
|
@@ -152,9 +254,12 @@ class Qbead { | |
| float rbuffer[3]; | ||
| float x, y, z, rx, ry, rz; // filtered and raw acc, in units of g | ||
| float t_acc, p_acc; // theta and phi according to gravity | ||
| float T_imu; // last update from the IMU | ||
| float T_imu; // last update from the IMU | ||
|
|
||
| float t_ble, p_ble; // BLE theta and phi | ||
|
|
||
| State state; | ||
|
Contributor
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. We do not need
Contributor
Author
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Hello, thank you for your feedback. However, I dont fully understand, so the state object should not be part of the qbead object? I was thinking that each instance of qbead should include a state, that will manage all the state related functionality. As I understand now, state and qbead should be independent objects, and if a user wants to assign a state to a qbead, first a state obj should be created and then assigned to the qbead? but then both the qbead and the state will hold values of x,y,z,theta,phy, where the values associated to qbead are the once from imu or ble and the values of state are custom assigned?
Contributor
Author
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. is it not possible to keep the state as an instance of qbead, and then if a user wants to assigned the values from the imu or ble to the state, do smth like this: eg. bead.state.setXYZ(bead.x, bead.y, bead.z) ?
Contributor
Author
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Like (as an example IMU_reader) now: void loop() { be something like: void loop() { then all the imu and ble readouts are stored in qbead and assigned to state when needed?
Contributor
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The issue is when we have more than one qbeads and want to have interesting entangled states. Is the "state" simply the state of the LEDs? If that is the case, it really should not be a separate objects to begin with and having it tightly coupled as you do is indeed the most reasonable thing to do. Is the "state" an object that represents a quantum state? Such an object can be plotted on the "output device" that is the LEDs of the qbead, but it is not the state of the LEDs itself. And we need a quantum state object in order to introduce the abstraction of gates and entanglement -- we do not need an LED state object. If we are going for this separation, then it makes more sense to be explicit, because it will not be always the case that there is one-to-one mapping of state-to-qbead -- we might want to have entangled states on multiple qbeads for instance. |
||
|
|
||
| float t_ble, p_ble; // theta and phi | ||
|
nynzzz marked this conversation as resolved.
|
||
| uint32_t c_ble; // color | ||
|
|
||
| static void ble_callback_color(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) { | ||
|
|
@@ -175,11 +280,15 @@ class Qbead { | |
| clear(); | ||
| setBrightness(10); | ||
|
|
||
| state.setXYZ(0, 0, 1); // Ensure the state starts pointing along the z-axis | ||
|
|
||
| Serial.println("qbead on XIAO BLE Sense + LSM6DS3 compiled on " __DATE__ " at " __TIME__); | ||
| if (!imu.begin()) { | ||
| Serial.println("IMU error"); | ||
| uint16_t imuResult = imu.begin(); | ||
| if (imuResult != 0) { | ||
| Serial.print("IMU error: "); | ||
| Serial.println(imuResult); | ||
| } else { | ||
| Serial.println("IMU OK"); | ||
| Serial.println("IMU OK"); | ||
| } | ||
|
nynzzz marked this conversation as resolved.
|
||
|
|
||
| Bluefruit.begin(QB_MAX_PRPH_CONNECTION, 0); | ||
|
|
@@ -203,9 +312,9 @@ class Qbead { | |
| blecharacc.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY); | ||
| blecharacc.setPermission(SECMODE_OPEN, SECMODE_OPEN); | ||
| blecharacc.setUserDescriptor("xyz acceleration"); | ||
| blecharacc.setFixedLen(3*sizeof(float)); | ||
| blecharacc.setFixedLen(3 * sizeof(float)); | ||
| blecharacc.begin(); | ||
| blecharacc.write(zerobuffer20, 3*sizeof(float)); | ||
| blecharacc.write(zerobuffer20, 3 * sizeof(float)); | ||
| startBLEadv(); | ||
| } | ||
|
|
||
|
|
@@ -221,13 +330,13 @@ class Qbead { | |
| leg = nlegs - leg; // invert direction for the phi angle, because the PCB is set up as a left-handed coordinate system | ||
| leg = leg % nlegs; | ||
| if (leg == 0) { | ||
| pixels.setPixelColor(pixel, color); | ||
| pixels.setPixelColor(pixel, color); | ||
| } else if (pixel == 0) { | ||
| pixels.setPixelColor(0, color); | ||
| pixels.setPixelColor(0, color); | ||
| } else if (pixel == 6) { | ||
| pixels.setPixelColor(6, color); | ||
| pixels.setPixelColor(6, color); | ||
| } else { | ||
| pixels.setPixelColor(7 + (leg - 1) * (nsections - 1) + pixel - 1, color); | ||
| pixels.setPixelColor(7 + (leg - 1) * (nsections - 1) + pixel - 1, color); | ||
| } | ||
| } | ||
|
|
||
|
|
@@ -239,17 +348,17 @@ class Qbead { | |
| if (theta < 0 || theta > 180 || phi < 0 || phi > 360) { | ||
| return; | ||
| } | ||
| float theta_section = theta / theta_quant; | ||
|
Contributor
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This is a bit weird. Why is this change made? This does not use the input function parameter anymore. This can probably look better as a new function |
||
| float theta_section = state.getTheta() / theta_quant; | ||
| if (theta_section < 0.5) { | ||
| setLegPixelColor(0, 0, color); | ||
| } else if (theta_section > nsections - 0.5) { | ||
| setLegPixelColor(0, nsections, color); | ||
| } else { | ||
| float phi_leg = phi / phi_quant; | ||
| float phi_leg = state.getPhi() / phi_quant; | ||
| int theta_int = theta_section + 0.5; | ||
| theta_int = theta_int > nsections - 1 ? nsections - 1 : theta_int; // to avoid precision issues near the end of the range | ||
| int phi_int = phi_leg + 0.5; | ||
| phi_int = phi_int > nlegs - 1 ? 0 : phi_int; | ||
| phi_int = phi_int % nlegs; | ||
| setLegPixelColor(phi_int, theta_int, color); | ||
| } | ||
| } | ||
|
|
@@ -261,9 +370,9 @@ class Qbead { | |
| float theta_section = theta / theta_quant; | ||
| float phi_leg = phi / phi_quant; | ||
| int theta_int = theta_section + 0.5; | ||
| theta_int = theta_int > nsections - 1 ? nsections - 1 : theta_int; // to avoid precision issues near the end of the range | ||
| theta_int = theta_int > nsections - 1 ? nsections - 1 : theta_int; // to avoid precision issues near the end of the range | ||
| int phi_int = phi_leg + 0.5; | ||
| phi_int = phi_int > nlegs - 1 ? 0 : phi_int; | ||
| phi_int = phi_int % nlegs; | ||
|
nynzzz marked this conversation as resolved.
|
||
|
|
||
| float p = (theta_section - theta_int); | ||
| int theta_direction = sign(p); | ||
|
|
@@ -273,11 +382,11 @@ class Qbead { | |
| q = q * q; | ||
|
|
||
| uint8_t rc = redch(c); | ||
| uint8_t bc = bluech(c); | ||
| uint8_t gc = greench(c); | ||
| uint8_t bc = bluech(c); | ||
|
Comment on lines
384
to
+386
Contributor
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. formatting and reordering comment: if moving this is not necessary, please avoid doing it in a "feature" PR, so that git-blame is easy to use in the future. I also generally prefer the order to be the standard RGB. |
||
|
|
||
| setLegPixelColor(phi_int, theta_int, color(q * rc, q * bc, q * gc)); | ||
| setLegPixelColor(phi_int, theta_int + theta_direction, color(p * rc, p * bc, p * gc)); | ||
| setLegPixelColor(phi_int, theta_int, color(q * rc, q * gc, q * bc)); | ||
| setLegPixelColor(phi_int, theta_int + theta_direction, color(p * rc, p * gc, p * bc)); | ||
| } | ||
|
|
||
| void readIMU() { | ||
|
|
@@ -366,4 +475,4 @@ Qbead *Qbead::singletoninstance = nullptr; | |
|
|
||
| } // end namespace | ||
|
|
||
| #endif // QBEAD_H | ||
| #endif // QBEAD_H | ||
|
Contributor
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. avoid these end-of-file modifications |
||
Uh oh!
There was an error while loading. Please reload this page.