diff --git a/examples/Decoherence/Decoherence.ino b/examples/Decoherence/Decoherence.ino new file mode 100644 index 0000000..8a034d0 --- /dev/null +++ b/examples/Decoherence/Decoherence.ino @@ -0,0 +1,86 @@ +#include + +Qbead::Qbead bead; +int rotationState = 0; +uint32_t stateColor = color(255, 255, 255); +uint32_t decoherenceColor = color(122, 0, 122); +const bool toggleAnimationOn = 1; +Qbead::Coordinates oldCoordinates(0, 0, 1); +int t = 0; +bool wasFrozen = false; + +void setup() +{ + bead.begin(); + bead.setBrightness(25); + Serial.println("testing all pixels discretely"); + for (int i = 0; i < bead.pixels.numPixels(); i++) + { + bead.pixels.setPixelColor(i, color(255, 255, 255)); + bead.pixels.show(); + delay(5); + } + Serial.println("testing smooth transition between pixels"); + for (int phi = 0; phi < 360; phi += 30) + { + for (int theta = 0; theta < 180; theta += 3) + { + bead.clear(); + bead.setBloch_deg(theta, phi, colorWheel_deg(phi)); + bead.show(); + } + } + Serial.println("starting inertial tracking"); + oldCoordinates = bead.state.getCoordinates(); + t = millis(); +} + +void loop() +{ + bead.readIMU(true); + stateColor = color(255, 255, 255); + Serial.print("rotationState: "); + Serial.println(rotationState); + if (bead.frozen) + { + stateColor = color(122, 122, 0); + wasFrozen = true; + } + else + { + if (wasFrozen) + { + wasFrozen = false; + oldCoordinates = bead.state.getCoordinates(); + } + rotationState = bead.checkMotion(); + if (rotationState != 0) + { + bead.frozen = true; + bead.T_freeze = millis(); + } + float phi = bead.state.getCoordinates().phi(); + int dt = millis() - t; + float randInt = random(200, 10000); + phi += dt / randInt; + if (phi > 2 * PI) + { + phi -= 2 * PI; + } + Qbead::Coordinates newCoordinates(bead.state.getCoordinates().theta(), phi); + bead.state.setCoordinates(newCoordinates); + bead.visualState = bead.state.getCoordinates(); + } + t = millis(); + bead.animateTo(rotationState, 2000); + if (bead.T_led + 20 < millis()) { + bead.T_led = millis(); + bead.clear(); + if (!bead.frozen) { + bead.setLed(oldCoordinates, decoherenceColor); + } + bead.showAxis(); + bead.setLed(bead.visualState, stateColor, 1); + bead.show(); + } +} \ No newline at end of file diff --git a/examples/IMU_reader/IMU_reader.ino b/examples/IMU_reader/IMU_reader.ino index 72ad22c..6d7e745 100644 --- a/examples/IMU_reader/IMU_reader.ino +++ b/examples/IMU_reader/IMU_reader.ino @@ -25,6 +25,6 @@ void setup() { void loop() { bead.readIMU(); bead.clear(); - bead.setBloch_deg_smooth(bead.t_acc, bead.p_acc, color(255, 0, 255)); + bead.setLed(Qbead::Coordinates(bead.gravityVector), color(255, 0, 255), true); bead.show(); } \ No newline at end of file diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino new file mode 100644 index 0000000..08d97fb --- /dev/null +++ b/examples/PauliGate_detection/PauliGate_detection.ino @@ -0,0 +1,54 @@ +#include + +Qbead::Qbead bead; +int rotationState = 0; +uint32_t stateColor = color(255, 255, 255); +const bool toggleAnimationOn = 1; + +void setup() { + bead.begin(); + bead.setBrightness(25); // way too bright + Serial.println("testing all pixels discretely"); + for (int i = 0; i < bead.pixels.numPixels(); i++) { + bead.pixels.setPixelColor(i, color(255, 255, 255)); + bead.pixels.show(); + delay(5); + } + Serial.println("testing smooth transition between pixels"); + for (int phi = 0; phi < 360; phi += 30) { + for (int theta = 0; theta < 180; theta += 3) { + bead.setBloch_deg(theta, phi, colorWheel_deg(phi)); + bead.show(); + } + } + Serial.println("starting inertial tracking"); +} + +void loop() { + bead.readIMU(false); + stateColor = color(255, 255, 255); + Serial.print("rotationState: "); + Serial.println(rotationState); + if (bead.frozen) + { + stateColor = color(122, 122, 0); + } + else + { + rotationState = bead.checkMotion(); + if (rotationState != 0) + { + bead.frozen = true; + bead.T_freeze = millis(); + } + } + bead.animateTo(rotationState, 2000); + // The leds can only be updated every 20ms + if (bead.T_led + 20 < millis()) { + bead.T_led = millis(); + bead.clear(); + bead.showAxis(); + bead.setLed(bead.visualState, stateColor); + bead.show(); + } +} \ No newline at end of file diff --git a/generateCoordinates.py b/generateCoordinates.py new file mode 100644 index 0000000..3273a45 --- /dev/null +++ b/generateCoordinates.py @@ -0,0 +1,46 @@ +# This is a script to convert the pick and place file to a coordinates map +# This code is very much not optimized, but it does its job. + +import math + +inputFile = "../pick-and-place.csv" +outputFile = "../pick-and-place-coordinates.csv" + +lines = [] + +with open(inputFile, 'r') as file: + for line in file: + if line.strip(): # Check if the line is not empty + lines.append(line.strip()) + +x0 = float(lines[0].split(',')[3]) +y0 = float(lines[0].split(',')[4]) + +coordinates = [] +for line in lines: + parts = line.split(',') + if len(parts) >= 2: + x = float(parts[3]) - x0 + y = float(parts[4]) - y0 + coordinates.append((x, y)) + +# Convert to spherical coordinates +spherical_coordinates = [] +for x, y in coordinates: + r = (x**2 + y**2)**0.5 + phi = math.atan2(y, x) # angle in radians + if phi < 0: + phi += 2 * math.pi # Normalize angle to [0, 2π) + spherical_coordinates.append((r, phi)) + +# scale r from 0 to pi +scaled_coordinates = [] +for r, phi in spherical_coordinates: + scaled_r = r / max(r for r, _ in spherical_coordinates) * math.pi + scaled_coordinates.append((round(scaled_r, 2), round(phi, 2))) + +# Write to output file +with open(outputFile, 'w') as file: + for theta, phi in scaled_coordinates: + file.write(f"Coordinates({theta}, {phi}),\n") +print(f"Coordinates written to {outputFile}") diff --git a/src/Qbead.h b/src/Qbead.h index 915c258..a01179f 100644 --- a/src/Qbead.h +++ b/src/Qbead.h @@ -1,28 +1,30 @@ #ifndef QBEAD_H #define QBEAD_H - #include #include #include #include - +#include #include +using namespace Eigen; + // default configs -#define QB_LEDPIN 0 +#define QB_LEDPIN 10 #define QB_PIXELCONFIG NEO_BRG + NEO_KHZ800 -#define QB_NSECTIONS 6 -#define QB_NLEGS 12 #define QB_IMU_ADDR 0x6A -#define QB_IX 0 -#define QB_IY 2 -#define QB_IZ 1 +#define QB_IX 1 +#define QB_IY 0 +#define QB_IZ 2 #define QB_SX 0 #define QB_SY 0 #define QB_SZ 1 - +#define GYRO_GATE_THRESHOLD 8 +#define QB_PIXEL_COUNT 62 #define QB_MAX_PRPH_CONNECTION 2 +#define T_ACC 100000 +#define T_GYRO 10000 const uint8_t QB_UUID_SERVICE[] = {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3}; @@ -32,12 +34,16 @@ const uint8_t QB_UUID_SPH_CHAR[] = {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+2,0x1f,0x0c,0xe3}; const uint8_t QB_UUID_ACC_CHAR[] = {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+3,0x1f,0x0c,0xe3}; +const uint8_t QB_UUID_GYR_CHAR[] = +{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+4,0x1f,0x0c,0xe3}; const uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; +const std::complexi(0, 1); // TODO manage namespaces better +// The setPixelColor switches blue and green static uint32_t color(uint8_t r, uint8_t g, uint8_t b) { - return ((uint32_t)r << 16) | ((uint32_t)g << 8) | b; + return ((uint32_t)r << 16) | ((uint16_t)b << 8) | g; } static uint8_t redch(uint32_t rgb) { @@ -45,11 +51,11 @@ static uint8_t redch(uint32_t rgb) { } static uint8_t greench(uint32_t rgb) { - return (0x00ff00 & rgb) >> 8; + return 0x0000ff & rgb; } static uint8_t bluech(uint32_t rgb) { - return 0x0000ff & rgb; + return (0x00ff00 & rgb) >> 8; } uint32_t colorWheel(uint8_t wheelPos) { @@ -109,34 +115,261 @@ void connect_callback(uint16_t conn_handle) Serial.println(central_name); } +// In rads +void sphericalToCartesian(float theta, float phi, float& x, float& y, float& z) +{ + // Normalize yaw to be between 0 and 2*PI + phi = fmod(phi, 2 * PI); + if (phi < 0) + { + phi += 2 * PI; + } + if (!checkThetaAndPhi(theta * 180 / PI, phi * 180 / PI)) + { + Serial.print("Theta or Phi out of range when creating coordinates class, initializing as 1"); + Serial.print("Theta: "); + Serial.print(theta); + Serial.print("Phi: "); + Serial.println(phi); + x = 0; + y = 0; + z = 1; + return; + } + + x = sin(theta) * cos(phi); + y = sin(theta) * sin(phi); + z = cos(theta); +} + +// Tap detection +LSM6DS3 myIMU(I2C_MODE, QB_IMU_ADDR); // Create an instance of the IMU +uint8_t interruptCount = 0; // Amount of received interrupts +uint8_t prevInterruptCount = 0; // Interrupt Counter from last loop + +void setupTapInterrupt() { + uint8_t error = 0; + uint8_t dataToWrite = 0; + + // Double Tap Config + myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x60); + myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, 0x8E);// INTERRUPTS_ENABLE, SLOPE_FDS + myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_THS_6D, 0x6C); + myIMU.writeRegister(LSM6DS3_ACC_GYRO_INT_DUR2, 0x7F); + myIMU.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_THS, 0x80); + myIMU.writeRegister(LSM6DS3_ACC_GYRO_MD1_CFG, 0x08); +} + +void int1ISR() +{ + interruptCount++; +} + namespace Qbead { +class Coordinates +{ +public: + Vector3d v; + + Coordinates(float argx, float argy, float argz) + { + v = Vector3d(argx, argy, argz); + v.normalize(); + } + + // In rads + Coordinates(float theta, float phi) + { + float x, y, z = 0; + sphericalToCartesian(theta, phi, x, y, z); + v = Vector3d(x, y, z); + } + + Coordinates(Vector3d vector) + { + v = vector; + v.normalize(); + } + + // In rads + float theta() + { + return acos(v(2)); + } + + // In rads + float phi() + { + return atan2(v(1), v(0)); + } + + Vector2cf stateVector2D() + { + std::complex alpha = cos(theta()/2); + std::complex beta = exp(i*phi()) * sin(theta()/2); + return {alpha, beta}; + } + + float dist(Vector3d other) const + { + Vector3d diff = v - other; + return diff.norm(); + } + + void set(float argx, float argy, float argz) + { + v = Vector3d(argx, argy, argz); + v.normalize(); + } + + // in rads + void set(float theta, float phi) + { + float x, y, z = 0; + sphericalToCartesian(theta, phi, x, y, z); + v = Vector3d(x, y, z); + } + + void set(Vector3d vector) { + v = vector; + v.normalize(); + } + + // in rads + void setTheta(float theta) + { + set(theta, phi()); + } + + // in rads + void setPhi(float phi) + { + set(theta(), phi); + } +}; + +class QuantumState +{ +private: + Coordinates stateCoordinates; + +public: + QuantumState(Coordinates argStateCoordinates) : stateCoordinates(argStateCoordinates) {} + QuantumState() : stateCoordinates(0, 0, 1) {} + + void setCoordinates(Coordinates argStateCoordinates) + { + stateCoordinates.set(argStateCoordinates.v); + } + + Coordinates getCoordinates() + { + return stateCoordinates; + } + + void collapse() + { + const float a = (stateCoordinates.v(2) + 1) / 2; // probability of measuring |0> + if (a < 0.0001) { + stateCoordinates.set(0, 0, -1); + return; + } else if (a > 0.9999) { + stateCoordinates.set(0, 0, 1); + return; + } + const bool is1 = random(0, 100) <= a * a * 100; + this->stateCoordinates.set(0, 0, is1 ? 1 : -1); + } + + void applyGate(Matrix2cf gate) + { + Vector2cf stateVector = stateCoordinates.stateVector2D(); + stateVector = gate * stateVector; + stateVector.normalize(); + stateCoordinates.set(2*acos(abs(stateVector.x())), arg(stateVector.y()) - arg(stateVector.x())); + } + + void applyGateType(uint16_t gateType, float rotationDegree = PI) + { + switch (gateType) + { + case 1: + gateX(-rotationDegree); + break; + case 2: + gateY(-rotationDegree); + break; + case 3: + gateZ(rotationDegree); + break; + case 4: + gateX(rotationDegree); + break; + case 5: + gateY(rotationDegree); + break; + case 6: + gateZ(-rotationDegree); + break; + case 7: + gateH(rotationDegree); + break; + default: + break; + } + } + + // Rotate PI around the x axis + void gateX(float rotationDegree = PI) + { + Matrix2cf gateMatrix; + gateMatrix << cos(rotationDegree / 2.0f), -sin(rotationDegree / 2.0f) * i, + -sin(rotationDegree / 2.0f) * i, cos(rotationDegree / 2.0f); // global phase differs from pauli gates but this doesn't matter for bloch sphere + applyGate(gateMatrix); + } + + // Rotate PI around the y axis + void gateZ(float rotationDegree = PI) + { + Matrix2cf gateMatrix; + gateMatrix << exp(-i * rotationDegree / 2.0f), 0, + 0, exp(i * rotationDegree / 2.0f); + applyGate(gateMatrix); + } + + // Rotate PI around the z axis + void gateY(float rotationDegree = PI) + { + Matrix2cf gateMatrix; + gateMatrix << cos(rotationDegree / 2.0f), -sin(rotationDegree / 2.0f), + sin(rotationDegree / 2.0f), cos(rotationDegree / 2.0f); + applyGate(gateMatrix); + } + + // Rotate PI around the xz axis + void gateH(float rotationDegree = PI) + { + Matrix2cf gateMatrix; + gateMatrix << (cos(rotationDegree / 2.0f) - i * sin(rotationDegree / 2.0f) / sqrt(2.0f)), -i * sin(rotationDegree / 2.0f) / sqrt(2.0f), + -i * sin(rotationDegree / 2.0f) / sqrt(2.0f), (cos(rotationDegree / 2.0f) + i * sin(rotationDegree / 2.0f) / sqrt(2.0f)); + applyGate(gateMatrix); + } +}; + class Qbead { public: Qbead(const uint16_t pin00 = QB_LEDPIN, const uint16_t pixelconfig = QB_PIXELCONFIG, - const uint16_t nsections = QB_NSECTIONS, - const uint16_t nlegs = QB_NLEGS, - const uint8_t imu_addr = QB_IMU_ADDR, - const uint8_t ix = QB_IX, - const uint8_t iy = QB_IY, - const uint8_t iz = QB_IZ, - const bool sx = QB_SX, - const bool sy = QB_SY, - const bool sz = QB_SZ) + const uint8_t imu_addr = QB_IMU_ADDR) : imu(LSM6DS3(I2C_MODE, imu_addr)), - pixels(Adafruit_NeoPixel(nlegs * (nsections - 1) + 2, pin00, pixelconfig)), - nsections(nsections), - nlegs(nlegs), - theta_quant(180 / nsections), - phi_quant(360 / nlegs), - ix(ix), iy(iy), iz(iz), - sx(sx), sy(sy), sz(sz), + pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig)), bleservice(QB_UUID_SERVICE), blecharcol(QB_UUID_COL_CHAR), blecharsph(QB_UUID_SPH_CHAR), - blecharacc(QB_UUID_ACC_CHAR) - {} + blecharacc(QB_UUID_ACC_CHAR), + blechargyr(QB_UUID_GYR_CHAR) + {} static Qbead *singletoninstance; // we need a global singleton static instance because bluefruit callbacks do not support context variables -- thankfully this is fine because there is indeed only one Qbead in existence at any time @@ -147,22 +380,92 @@ class Qbead { BLECharacteristic blecharcol; BLECharacteristic blecharsph; BLECharacteristic blecharacc; - uint8_t connection_count = 0; - - const uint8_t nsections; - const uint8_t nlegs; - const uint8_t theta_quant; - const uint8_t phi_quant; - const uint8_t ix, iy, iz; - const bool sx, sy, sz; - 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 + BLECharacteristic blechargyr; + + float rbuffer[3], rgyrobuffer[3]; float T_imu; // last update from the IMU + float T_freeze = 0; + float T_shaking = 0; + float T_led = 0; // last update to the LEDs + bool frozen = false; // frozen means that there is an animation in progress + bool shakingState = false; // if ShakingState is 1 detected shaking and if shaking keeps happening randomising state + QuantumState state = QuantumState(Coordinates(-0.866, 0.25, -0.433)); + Coordinates visualState = Coordinates(-0.866, 0.25, -0.433); + Vector3d gravityVector = Vector3d(0, 0, 1); + Vector3d gyroVector = Vector3d(0, 0, 1); + float yaw = 0; + float dt = 0; // time difference between the last two IMU updates float t_ble, p_ble; // theta and phi as sent over BLE connection uint32_t c_ble = 0xffffff; // color as sent over BLE connection + // led map index to Coordinates + // This map is for the first version of the flex-pcb + Coordinates led_map_v1[62] = { + Coordinates(-1, -0, -0), + Coordinates(-0.866, 0, -0.5), + Coordinates(-0.5, 0, -0.866), + Coordinates(-0, 0, -1), + Coordinates(0.5, 0, -0.866), + Coordinates(0.866, 0, -0.5), + Coordinates(1, 0, 0), + Coordinates(-0.866, 0.25, -0.433), + Coordinates(-0.5, 0.433, -0.75), + Coordinates(-0, 0.5, -0.866), + Coordinates(0.5, 0.433, -0.75), + Coordinates(0.866, 0.25, -0.433), + Coordinates(-0.866, 0.433, -0.25), + Coordinates(-0.5, 0.75, -0.433), + Coordinates(-0, 0.866, -0.5), + Coordinates(0.5, 0.75, -0.433), + Coordinates(0.866, 0.433, -0.25), + Coordinates(-0.866, 0.5, 0), + Coordinates(-0.5, 0.866, 0), + Coordinates(-0, 1, 0), + Coordinates(0.5, 0.866, 0), + Coordinates(0.866, 0.5, 0), + Coordinates(-0.866, 0.433, 0.25), + Coordinates(-0.5, 0.75, 0.433), + Coordinates(-0, 0.866, 0.5), + Coordinates(0.5, 0.75, 0.433), + Coordinates(0.866, 0.433, 0.25), + Coordinates(-0.866, 0.25, 0.433), + Coordinates(-0.5, 0.433, 0.75), + Coordinates(-0, 0.5, 0.866), + Coordinates(0.5, 0.433, 0.75), + Coordinates(0.866, 0.25, 0.433), + Coordinates(-0.866, -0, 0.5), + Coordinates(-0.5, -0, 0.866), + Coordinates(-0, -0, 1), + Coordinates(0.5, -0, 0.866), + Coordinates(0.866, -0, 0.5), + Coordinates(-0.866, -0.25, 0.433), + Coordinates(-0.5, -0.433, 0.75), + Coordinates(-0, -0.5, 0.866), + Coordinates(0.5, -0.433, 0.75), + Coordinates(0.866, -0.25, 0.433), + Coordinates(-0.866, -0.433, 0.25), + Coordinates(-0.5, -0.75, 0.433), + Coordinates(-0, -0.866, 0.5), + Coordinates(0.5, -0.75, 0.433), + Coordinates(0.866, -0.433, 0.25), + Coordinates(-0.866, -0.5, -0), + Coordinates(-0.5, -0.866, -0), + Coordinates(-0, -1, -0), + Coordinates(0.5, -0.866, -0), + Coordinates(0.866, -0.5, -0), + Coordinates(-0.866, -0.433, -0.25), + Coordinates(-0.5, -0.75, -0.433), + Coordinates(-0, -0.866, -0.5), + Coordinates(0.5, -0.75, -0.433), + Coordinates(0.866, -0.433, -0.25), + Coordinates(-0.866, -0.25, -0.433), + Coordinates(-0.5, -0.433, -0.75), + Coordinates(-0, -0.5, -0.866), + Coordinates(0.5, -0.433, -0.75), + Coordinates(0.866, -0.25, -0.433), + }; + static void ble_callback_color(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) { Serial.println("[INFO]{BLE} Received a write on the color characteristic"); singletoninstance->c_ble = (data[2] << 16) | (data[1] << 8) | data[0]; @@ -180,12 +483,27 @@ class Qbead { Serial.println(singletoninstance->p_ble); } + void startAccelerometer() { + // BLE Characteristic IMU xyz accelerometer readout + blecharacc.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY); + blecharacc.setPermission(SECMODE_OPEN, SECMODE_OPEN); + blecharacc.setUserDescriptor("xyz acceleration"); + blecharacc.setFixedLen(3*sizeof(float)); + blecharacc.begin(); + blecharacc.write(zerobuffer20, 3*sizeof(float)); + } + void begin() { singletoninstance = this; Serial.begin(9600); - while (!Serial); // TODO some form of warning or a way to give up if Serial never becomes available + for (int waitCount = 0; waitCount < 50; waitCount++) + { + if (Serial) {break;} + delay(100); + } pixels.begin(); + T_led = millis(); clear(); setBrightness(10); @@ -217,14 +535,19 @@ class Qbead { blecharsph.setWriteCallback(ble_callback_theta_phi); blecharsph.begin(); blecharsph.write(zerobuffer20, 2); - // BLE Characteristic IMU xyz readout - blecharacc.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY); - blecharacc.setPermission(SECMODE_OPEN, SECMODE_OPEN); - blecharacc.setUserDescriptor("xyz acceleration"); - blecharacc.setFixedLen(3*sizeof(float)); - blecharacc.begin(); - blecharacc.write(zerobuffer20, 3*sizeof(float)); + // BLE Characteristic IMU xyz gyroscope readout + blechargyr.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY); + blechargyr.setPermission(SECMODE_OPEN, SECMODE_OPEN); + blechargyr.setUserDescriptor("xyz gyroscope"); + blechargyr.setFixedLen(3*sizeof(float)); + blechargyr.begin(); + blechargyr.write(zerobuffer20, 3*sizeof(float)); startBLEadv(); + + // Tap detection + setupTapInterrupt(); + pinMode(PIN_LSM6DS3TR_C_INT1, INPUT); + attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING); } void clear() { @@ -235,113 +558,256 @@ class Qbead { pixels.show(); } - void setLegPixelColor(int leg, int pixel, uint32_t color) { - 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); - } else if (pixel == 0) { - pixels.setPixelColor(0, color); - } else if (pixel == 6) { - pixels.setPixelColor(6, color); - } else { - pixels.setPixelColor(7 + (leg - 1) * (nsections - 1) + pixel - 1, color); + void setBrightness(uint8_t b) { + pixels.setBrightness(b); + } + + void setLed(Coordinates coordinates, uint32_t color, int leds = 1) { + float theta = coordinates.theta() * 180 / PI; + float phi = coordinates.phi() * 180 / PI; + if (phi < 0) { + phi += 360; } + setBloch_deg(theta, phi, color, leds); } - void setBrightness(uint8_t b) { - pixels.setBrightness(b); + void showAxis() { + setLed(Coordinates(1, 0, 0), color(0, 0, 122)); + setLed(Coordinates(-1, 0, 0), color(0, 0, 122)); + setLed(Coordinates(0, 1, 0), color(0, 0, 122)); + setLed(Coordinates(0, -1, 0), color(0, 0, 122)); + setLed(Coordinates(0, 0, 1), color(0, 255, 0)); + setLed(Coordinates(0, 0, -1), color(255, 0, 0)); } - void setBloch_deg(float theta, float phi, uint32_t color) { - if (!checkThetaAndPhi(theta, phi)) return; - float theta_section = theta / theta_quant; - if (theta_section < 0.5) { - setLegPixelColor(0, 0, color); - } else if (theta_section > nsections - 0.5) { - setLegPixelColor(0, nsections, color); - } else { - int theta_int = min(nsections - 1, round(theta_section)); // to avoid precision issues near the end of the range - int phi_int = round(phi / phi_quant); - phi_int = phi_int > nlegs - 1 ? 0 : phi_int; - setLegPixelColor(phi_int, theta_int, color); + // in rads + float getDistToLed(float theta, float phi, int index) { + const Coordinates led = led_map_v1[index]; + const Coordinates reference(theta, phi); + return led.dist(reference.v); + } + + // Single bit is lit up on the Bloch sphere + void setBloch_deg(float theta, float phi, uint32_t c, int leds = 1) { + int index[leds]; + float dist[leds]; + for (int i = 0; i < leds; i++) { + index[i] = -1; + dist[i] = 1000; + } + for (int i = 0; i < QB_PIXEL_COUNT; i++) { + float d = getDistToLed(theta * PI / 180, phi * PI / 180, i); + for (int j = 0; j < leds; j++) { + if (d < dist[j]) { + for (int k = leds - 1; k > j; k--) { + index[k] = index[k - 1]; + dist[k] = dist[k - 1]; + } + index[j] = i; + dist[j] = d; + break; + } + } + } + for (int i = 0; i < leds; i++) { + if (index[i] != -1) { + uint8_t r = redch(c); + uint8_t g = greench(c); + uint8_t b = bluech(c); + float p2 = pow(200, -dist[i]); + pixels.setPixelColor(index[i], color(p2 * r, p2 * g, p2 * b)); + } } } void setBloch_deg_smooth(float theta, float phi, uint32_t c) { - if (!checkThetaAndPhi(theta, phi)) return; - float theta_section = theta / theta_quant; - int theta_int = min(nsections - 1, round(theta_section)); // to avoid precision issues near the end of the range - int phi_int = round(phi / phi_quant); - phi_int = phi_int > nlegs - 1 ? 0 : phi_int; + setBloch_deg(theta, phi, c, 2); + } - float p = (theta_section - theta_int); - int theta_direction = sign(p); - p = abs(p); - float q = 1 - p; - p = p * p; - q = q * q; + void animateTo(uint8_t gate, uint16_t animationLength = 2000) + { + if (frozen) + { + prevInterruptCount = interruptCount; + } + else if (gate == 0) + { + return; + } + if (gate == 9) + { + visualState.set(state.getCoordinates().v); + } + if (gate == 8) + { + state.collapse(); + visualState.set(state.getCoordinates().v); + } + float T_new = millis(); + float delta = T_new - T_freeze; + if (delta > animationLength) + { + frozen = false; + state.applyGateType(gate); + Serial.println("Animation finished"); + return; + } + float d = delta * PI / float(animationLength); + QuantumState from = state; + from.applyGateType(gate, d); + visualState.set(from.getCoordinates().v); + } - uint8_t rc = redch(c); - uint8_t bc = bluech(c); - uint8_t gc = greench(c); + bool detectShaking() + { + float totalAcceleration = gravityVector.norm(); + if (shakingState) + { + float newTime = millis(); + float shakingCounter = newTime - T_shaking; + if (shakingCounter < 300) + { + return false; + } + if (totalAcceleration > 11) + { + Serial.println("Randomizing"); + float randomTheta = (random(0, 1000)/1000.0f) * PI; + float randomPhi = (random(0, 1000)/500.0f) * PI; + state.setCoordinates(Coordinates(randomTheta, randomPhi)); + setLed(state.getCoordinates(), color(255, 0, 255)); + shakingState = false; + return true; + } + if (shakingCounter > 800) + { + shakingState = false; + } + return false; + } + if (totalAcceleration > 11) + { + Serial.print("Detected shaking turning on shakingState, acc length: "); + Serial.println(totalAcceleration); + shakingState = true; + T_shaking = millis(); + } + return false; + } + + int checkMotion() + { + if (frozen) + { + return 0; + } + frozen = true; + T_freeze = micros(); + if (detectShaking()) + { + return 9; + } + if (shakingState) + { + frozen = false; + return 0; + } + // Handle tap interrupt + if (interruptCount > prevInterruptCount) + { + uint8_t tapStatus = 0; + myIMU.readRegister(&tapStatus, LSM6DS3_ACC_GYRO_TAP_SRC); + prevInterruptCount = interruptCount; + + if (tapStatus & 0x01) + { + Serial.println("Collapsing"); + return 8; + } + else + { + Serial.println("Executing H gate"); + return 7; + } + } + // Handle shaking + for (int i = 0; i < 3; i++) + { + if (gyroVector[i] > GYRO_GATE_THRESHOLD) + { + return i + 1; // 1 = -x, 2 = -y, 3 = z + } + } + for (int i = 0; i < 3; i++) + { + if (gyroVector[i] < - GYRO_GATE_THRESHOLD) + { + return i + 4; // 4 = x, 5 = y, 6 = -z + } + } + frozen = false; + return 0; + } - 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)); + void writeToBLE(BLECharacteristic& destination, Vector3d vector) { + float buffer[3] = {(float)vector(0), (float)vector(1), (float)vector(2)}; + destination.write(buffer, 3 * sizeof(float)); + for (uint16_t conn_hdl = 0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++) + { + if (Bluefruit.connected(conn_hdl) && destination.notifyEnabled(conn_hdl)) + { + destination.notify(buffer, 3 * sizeof(float)); + } + } + } + + Vector3d getVectorFromBuffer(float *buffer) { + // calibration of imu because imu is not aligned with bloch sphere + float rx = (1 - 2 * QB_SX) * buffer[QB_IX]; + float ry = (1 - 2 * QB_SY) * buffer[QB_IY]; + float rz = (1 - 2 * QB_SZ) * buffer[QB_IZ]; + return Vector3d(rx, ry, rz); } void readIMU(bool print=true) { rbuffer[0] = imu.readFloatAccelX(); rbuffer[1] = imu.readFloatAccelY(); - rbuffer[2] = imu.readFloatAccelZ(); - rx = (1-2*sx)*rbuffer[ix]; - ry = (1-2*sy)*rbuffer[iy]; - rz = (1-2*sz)*rbuffer[iz]; + rbuffer[2] = imu.readFloatAccelZ(); + rgyrobuffer[0] = imu.readFloatGyroX(); + rgyrobuffer[1] = imu.readFloatGyroY(); + rgyrobuffer[2] = imu.readFloatGyroZ(); float T_new = micros(); - float delta = T_new - T_imu; + dt = T_new - T_imu; T_imu = T_new; - const float T = 100000; // 100 ms // TODO make the filter timeconstant configurable - if (delta > 100000) { - x = rx; - y = ry; - z = rz; - } else { - float d = delta/T; - x = d*rx+(1-d)*x; - y = d*ry+(1-d)*y; - z = d*rz+(1-d)*z; - } - t_acc = theta(x, y, z)*180/3.14159; - p_acc = phi(x, y)*180/3.14159; - if (p_acc<0) {p_acc+=360;}// to bring it to [0,360] range + Vector3d newGyro = getVectorFromBuffer(rgyrobuffer) * PI / 180; + float d = min(dt / float(T_GYRO), 1.0f); + gyroVector = d * newGyro + (1 - d) * gyroVector; // low pass filter + + Vector3d newGravity = getVectorFromBuffer(rbuffer); + d = min(dt / float(T_ACC), 1.0f); + gravityVector = d * newGravity + (1 - d) * gravityVector; + + yaw += gravityVector.dot(gyroVector); + yaw = fmod(yaw, 2 * PI); if (print) { - Serial.print(x); + Serial.print(gravityVector(0)); Serial.print("\t"); - Serial.print(y); + Serial.print(gravityVector(1)); Serial.print("\t"); - Serial.print(z); + Serial.print(gravityVector(2)); Serial.print("\t-1\t1\t"); - Serial.print(t_acc); + Serial.print(gyroVector(0)); Serial.print("\t"); - Serial.print(p_acc); - Serial.print("\t-360\t360\t"); - Serial.println(); + Serial.print(gyroVector(1)); + Serial.print("\t"); + Serial.println(gyroVector(2)); } - rbuffer[0] = x; - rbuffer[1] = y; - rbuffer[2] = z; - blecharacc.write(rbuffer, 3*sizeof(float)); - for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++) - { - if ( Bluefruit.connected(conn_hdl) && blecharacc.notifyEnabled(conn_hdl) ) - { - blecharacc.notify(rbuffer, 3*sizeof(float)); - } - } + writeToBLE(blecharacc, gravityVector); + writeToBLE(blechargyr, gyroVector); } void startBLEadv(void) @@ -379,4 +845,4 @@ Qbead *Qbead::singletoninstance = nullptr; } // end namespace -#endif // QBEAD_H +#endif // QBEAD_H \ No newline at end of file diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h new file mode 100644 index 0000000..3ddcf59 --- /dev/null +++ b/src_ESP32/QbeadESP32.h @@ -0,0 +1,998 @@ +#ifndef QBEAD_H +#define QBEAD_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Eigen; + +// default configs +#define QB_LEDPIN 21 +#define QB_PIXELCONFIG NEO_BRG + NEO_KHZ800 +#define QB_IMU_ADDR 0x69 +#define QB_IX 1 +#define QB_IY 0 +#define QB_IZ 2 +#define QB_SX 0 +#define QB_SY 0 +#define QB_SZ 1 +#define GYRO_GATE_THRESHOLD 12 +#define QB_PIXEL_COUNT 62 +#define QB_MAX_PRPH_CONNECTION 2 +#define T_ACC 100000 +#define T_GYRO 10000 +#define TAP_THRESHOLD_TIME 400 // Threshold for tap detection in milliseconds +#define TAP_THRESHOLD 5 // Threshold for tap detection in g/s +#define DEBOUNCE_TIME 50 // Debounce time in milliseconds + +const char QB_UUID_SERVICE[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68b043"; +// {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3}; +const char QB_UUID_COL_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68c043"; +// {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+1,0x1f,0x0c,0xe3}; +const char QB_UUID_SPH_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68d043"; +// {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+2,0x1f,0x0c,0xe3}; +const char QB_UUID_ACC_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68e043"; +// {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+3,0x1f,0x0c,0xe3}; +const char QB_UUID_GYR_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68f043"; +// {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+4,0x1f,0x0c,0xe3}; + +uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; +const std::complexi(0, 1); +// We need both because ICM_20948_I2C does not support changing the gyro range +ICM20948_WE imuWE; +ICM_20948_I2C imuI2C; + +// TODO manage namespaces better +// The setPixelColor switches blue and green +static uint32_t color(uint8_t r, uint8_t g, uint8_t b) { + return ((uint32_t)r << 16) | ((uint16_t)b << 8) | g; +} + +static uint8_t redch(uint32_t rgb) { + return rgb >> 16; +} + +static uint8_t greench(uint32_t rgb) { + return 0x0000ff & rgb; +} + +static uint8_t bluech(uint32_t rgb) { + return (0x00ff00 & rgb) >> 8; +} + +uint32_t colorWheel(uint8_t wheelPos) { + wheelPos = 255 - wheelPos; + if (wheelPos < 85) { + return color(255 - wheelPos * 3, 0, wheelPos * 3); + } + if (wheelPos < 170) { + wheelPos -= 85; + return color(0, wheelPos * 3, 255 - wheelPos * 3); + } + wheelPos -= 170; + return color(wheelPos * 3, 255 - wheelPos * 3, 0); +} + +uint32_t colorWheel_deg(float wheelPos) { + return colorWheel(wheelPos * 255 / 360); +} + +float sign(float x) { + if (x > 0) return +1; + else return -1; +} + +// z = cos(t) +// x = cos(p)sin(t) +// y = sin(p)sin(t) +// Return the angle in radians between the x-axis and the line to the point (x, y) +float phi(float x, float y) { + return atan2(y, x); +} + +float phi(float x, float y, float z) { + return phi(x, y); +} + +float theta(float x, float y, float z) { + float ll = x * x + y * y + z * z; + float l = sqrt(ll); + float theta = acos(z / l); + return theta; +} + +bool checkThetaAndPhi(float theta, float phi) { + return theta >= 0 && theta <= 180 && phi >= 0 && phi <= 360; +} + +// In rads +void sphericalToCartesian(float theta, float phi, float& x, float& y, float& z) +{ + // Normalize yaw to be between 0 and 2*PI + phi = fmod(phi, 2 * PI); + if (phi < 0) + { + phi += 2 * PI; + } + if (!checkThetaAndPhi(theta * 180 / PI, phi * 180 / PI)) + { + Serial.print("Theta or Phi out of range when creating coordinates class, initializing as 1"); + Serial.print("Theta: "); + Serial.print(theta); + Serial.print("Phi: "); + Serial.println(phi); + x = 0; + y = 0; + z = 1; + return; + } + + x = sin(theta) * cos(phi); + y = sin(theta) * sin(phi); + z = cos(theta); +} + +namespace Qbead { + +class Coordinates +{ +public: + Vector3d v; + + Coordinates(float argx, float argy, float argz) + { + v = Vector3d(argx, argy, argz); + v.normalize(); + } + + // In rads + Coordinates(float theta, float phi) + { + float x, y, z = 0; + sphericalToCartesian(theta, phi, x, y, z); + v = Vector3d(x, y, z); + } + + Coordinates(Vector3d vector) + { + v = vector; + v.normalize(); + } + + // In rads + float theta() + { + return acos(v(2)); + } + + // In rads + float phi() + { + return atan2(v(1), v(0)); + } + + Vector2cf stateVector2D() + { + std::complex alpha = cos(theta()/2); + std::complex beta = exp(i*phi()) * sin(theta()/2); + return {alpha, beta}; + } + + float dist(Vector3d other) const + { + Vector3d diff = v - other; + return diff.norm(); + } + + void set(float argx, float argy, float argz) + { + v = Vector3d(argx, argy, argz); + v.normalize(); + } + + // in rads + void set(float theta, float phi) + { + float x, y, z = 0; + sphericalToCartesian(theta, phi, x, y, z); + v = Vector3d(x, y, z); + } + + void set(Vector3d vector) { + v = vector; + v.normalize(); + } + + // in rads + void setTheta(float theta) + { + set(theta, phi()); + } + + // in rads + void setPhi(float phi) + { + set(theta(), phi); + } +}; + +class QuantumState +{ +private: + Coordinates stateCoordinates; + +public: + QuantumState(Coordinates argStateCoordinates) : stateCoordinates(argStateCoordinates) {} + QuantumState() : stateCoordinates(0, 0, 1) {} + + void setCoordinates(Coordinates argStateCoordinates) + { + stateCoordinates.set(argStateCoordinates.v); + } + + Coordinates getCoordinates() + { + return stateCoordinates; + } + + void collapse() + { + const float a = (stateCoordinates.v(2) + 1) / 2; // probability of measuring |0> + if (a < 0.0001) { + stateCoordinates.set(0, 0, -1); + return; + } else if (a > 0.9999) { + stateCoordinates.set(0, 0, 1); + return; + } + const bool is1 = random(0, 100) <= a * a * 100; + this->stateCoordinates.set(0, 0, is1 ? 1 : -1); + } + + void applyGate(Matrix2cf gate) + { + Vector2cf stateVector = stateCoordinates.stateVector2D(); + stateVector = gate * stateVector; + stateVector.normalize(); + stateCoordinates.set(2*acos(abs(stateVector.x())), arg(stateVector.y()) - arg(stateVector.x())); + } + + void applyGateType(uint16_t gateType, float rotationDegree = PI) + { + switch (gateType) + { + case 1: + gateX(-rotationDegree); + break; + case 2: + gateY(-rotationDegree); + break; + case 3: + gateZ(rotationDegree); + break; + case 4: + gateX(rotationDegree); + break; + case 5: + gateY(rotationDegree); + break; + case 6: + gateZ(-rotationDegree); + break; + case 7: + gateH(rotationDegree); + break; + default: + break; + } + } + + // Rotate PI around the x axis + void gateX(float rotationDegree = PI) + { + Matrix2cf gateMatrix; + gateMatrix << cos(rotationDegree / 2.0f), -sin(rotationDegree / 2.0f) * i, + -sin(rotationDegree / 2.0f) * i, cos(rotationDegree / 2.0f); // global phase differs from pauli gates but this doesn't matter for bloch sphere + applyGate(gateMatrix); + } + + // Rotate PI around the y axis + void gateZ(float rotationDegree = PI) + { + Matrix2cf gateMatrix; + gateMatrix << exp(-i * rotationDegree / 2.0f), 0, + 0, exp(i * rotationDegree / 2.0f); + applyGate(gateMatrix); + } + + // Rotate PI around the z axis + void gateY(float rotationDegree = PI) + { + Matrix2cf gateMatrix; + gateMatrix << cos(rotationDegree / 2.0f), -sin(rotationDegree / 2.0f), + sin(rotationDegree / 2.0f), cos(rotationDegree / 2.0f); + applyGate(gateMatrix); + } + + // Rotate PI around the xz axis + void gateH(float rotationDegree = PI) + { + Matrix2cf gateMatrix; + gateMatrix << (cos(rotationDegree / 2.0f) - i * sin(rotationDegree / 2.0f) / sqrt(2.0f)), -i * sin(rotationDegree / 2.0f) / sqrt(2.0f), + -i * sin(rotationDegree / 2.0f) / sqrt(2.0f), (cos(rotationDegree / 2.0f) + i * sin(rotationDegree / 2.0f) / sqrt(2.0f)); + applyGate(gateMatrix); + } +}; + +class Qbead { +public: + Qbead(const uint16_t pin00 = QB_LEDPIN, + const uint16_t pixelconfig = QB_PIXELCONFIG, + const uint8_t imu_addr = QB_IMU_ADDR) + : pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig)) + {} + + static Qbead *singletoninstance; // we need a global singleton static instance because bluefruit callbacks do not support context variables -- thankfully this is fine because there is indeed only one Qbead in existence at any time + + Adafruit_NeoPixel pixels; + + BLEServer* bleserver; + BLEService* bleservice; + BLECharacteristic* blecharcol; + BLECharacteristic* blecharsph; + BLECharacteristic* blecharacc; + BLECharacteristic* blechargyr; + BLEAdvertising* bleadvertising; + + float rbuffer[3], rgyrobuffer[3]; + float T_imu; // last update from the IMU + float T_freeze = 0; + float T_shaking = 0; + float T_led = 0; // last update to the LEDs + float t_ble, p_ble; // theta and phi as sent over BLE connection + uint32_t c_ble; + bool frozen = false; // frozen means that there is an animation in progress + bool shakingState = false; // if ShakingState is 1 detected shaking and if shaking keeps happening randomising state + QuantumState state = QuantumState(Coordinates(-0.866, 0.25, -0.433)); + Coordinates visualState = Coordinates(-0.866, 0.25, -0.433); + Vector3d gravityVector = Vector3d(0, 0, 1); + Vector3d oldGravityVector = Vector3d(0, 0, 1); + Vector3d gyroVector = Vector3d(0, 0, 1); + float lastTapTime = 0; + float lastDebounceTime = 0; // last time the tap was debounced + bool waitingForSecondTap = false; + float dt = 0; // time since the last IMU update + + // led map index to Coordinates + // This map is for the first version of the flex-pcb + Coordinates led_map_v1[62] = { + Coordinates(-1, -0, -0), + Coordinates(-0.866, 0, -0.5), + Coordinates(-0.5, 0, -0.866), + Coordinates(-0, 0, -1), + Coordinates(0.5, 0, -0.866), + Coordinates(0.866, 0, -0.5), + Coordinates(1, 0, 0), + Coordinates(-0.866, 0.25, -0.433), + Coordinates(-0.5, 0.433, -0.75), + Coordinates(-0, 0.5, -0.866), + Coordinates(0.5, 0.433, -0.75), + Coordinates(0.866, 0.25, -0.433), + Coordinates(-0.866, 0.433, -0.25), + Coordinates(-0.5, 0.75, -0.433), + Coordinates(-0, 0.866, -0.5), + Coordinates(0.5, 0.75, -0.433), + Coordinates(0.866, 0.433, -0.25), + Coordinates(-0.866, 0.5, 0), + Coordinates(-0.5, 0.866, 0), + Coordinates(-0, 1, 0), + Coordinates(0.5, 0.866, 0), + Coordinates(0.866, 0.5, 0), + Coordinates(-0.866, 0.433, 0.25), + Coordinates(-0.5, 0.75, 0.433), + Coordinates(-0, 0.866, 0.5), + Coordinates(0.5, 0.75, 0.433), + Coordinates(0.866, 0.433, 0.25), + Coordinates(-0.866, 0.25, 0.433), + Coordinates(-0.5, 0.433, 0.75), + Coordinates(-0, 0.5, 0.866), + Coordinates(0.5, 0.433, 0.75), + Coordinates(0.866, 0.25, 0.433), + Coordinates(-0.866, -0, 0.5), + Coordinates(-0.5, -0, 0.866), + Coordinates(-0, -0, 1), + Coordinates(0.5, -0, 0.866), + Coordinates(0.866, -0, 0.5), + Coordinates(-0.866, -0.25, 0.433), + Coordinates(-0.5, -0.433, 0.75), + Coordinates(-0, -0.5, 0.866), + Coordinates(0.5, -0.433, 0.75), + Coordinates(0.866, -0.25, 0.433), + Coordinates(-0.866, -0.433, 0.25), + Coordinates(-0.5, -0.75, 0.433), + Coordinates(-0, -0.866, 0.5), + Coordinates(0.5, -0.75, 0.433), + Coordinates(0.866, -0.433, 0.25), + Coordinates(-0.866, -0.5, -0), + Coordinates(-0.5, -0.866, -0), + Coordinates(-0, -1, -0), + Coordinates(0.5, -0.866, -0), + Coordinates(0.866, -0.5, -0), + Coordinates(-0.866, -0.433, -0.25), + Coordinates(-0.5, -0.75, -0.433), + Coordinates(-0, -0.866, -0.5), + Coordinates(0.5, -0.75, -0.433), + Coordinates(0.866, -0.433, -0.25), + Coordinates(-0.866, -0.25, -0.433), + Coordinates(-0.5, -0.433, -0.75), + Coordinates(-0, -0.5, -0.866), + Coordinates(0.5, -0.433, -0.75), + Coordinates(0.866, -0.25, -0.433), + }; + + void startAccelerometer() + { + blecharacc = bleservice->createCharacteristic(QB_UUID_ACC_CHAR, + BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY); + BLEDescriptor* pAccDesc = new BLEDescriptor("2901"); + pAccDesc->setValue("Accelerometer readout Characteristic"); + blecharacc->addDescriptor(pAccDesc); + blecharacc->addDescriptor(new BLE2902()); + blecharacc->setValue(zerobuffer20, 3*sizeof(float)); + } + + // TODO: Check when the new flex-pcb has arrived + Coordinates led_map_v2[107] = { + Coordinates(0.0, 0.0), + Coordinates(0.39, 4.97), + Coordinates(0.78, 4.97), + Coordinates(1.18, 5.08), + Coordinates(1.18, 4.87), + Coordinates(1.57, 4.89), + Coordinates(1.57, 5.06), + Coordinates(1.95, 5.04), + Coordinates(1.95, 4.91), + Coordinates(2.34, 4.97), + Coordinates(2.73, 4.97), + Coordinates(0.78, 4.45), + Coordinates(1.18, 4.55), + Coordinates(1.18, 4.35), + Coordinates(1.57, 4.37), + Coordinates(1.57, 4.53), + Coordinates(1.95, 4.51), + Coordinates(1.95, 4.39), + Coordinates(2.34, 4.45), + Coordinates(0.39, 3.93), + Coordinates(0.78, 3.93), + Coordinates(1.18, 4.03), + Coordinates(1.18, 3.82), + Coordinates(1.57, 3.84), + Coordinates(1.57, 4.01), + Coordinates(1.95, 3.99), + Coordinates(1.95, 3.87), + Coordinates(2.34, 3.93), + Coordinates(2.73, 3.93), + Coordinates(0.78, 3.4), + Coordinates(1.18, 3.3), + Coordinates(1.57, 3.32), + Coordinates(1.95, 3.34), + Coordinates(2.34, 3.4), + Coordinates(0.39, 2.88), + Coordinates(0.78, 2.88), + Coordinates(1.18, 2.98), + Coordinates(1.18, 2.78), + Coordinates(1.57, 2.8), + Coordinates(1.57, 2.96), + Coordinates(1.95, 2.94), + Coordinates(1.95, 2.82), + Coordinates(2.34, 2.88), + Coordinates(2.73, 2.88), + Coordinates(0.78, 2.36), + Coordinates(1.18, 2.46), + Coordinates(1.18, 2.25), + Coordinates(1.57, 2.27), + Coordinates(1.57, 2.44), + Coordinates(1.95, 2.42), + Coordinates(1.95, 2.29), + Coordinates(2.34, 2.36), + Coordinates(0.39, 1.83), + Coordinates(0.78, 1.83), + Coordinates(1.18, 1.93), + Coordinates(1.18, 1.73), + Coordinates(1.57, 1.75), + Coordinates(1.57, 1.92), + Coordinates(1.95, 1.89), + Coordinates(1.95, 1.77), + Coordinates(2.34, 1.83), + Coordinates(2.73, 1.83), + Coordinates(0.78, 1.31), + Coordinates(1.18, 1.41), + Coordinates(1.18, 1.21), + Coordinates(1.57, 1.23), + Coordinates(1.57, 1.39), + Coordinates(1.95, 1.37), + Coordinates(1.95, 1.25), + Coordinates(2.34, 1.31), + Coordinates(0.39, 0.79), + Coordinates(0.78, 0.79), + Coordinates(1.18, 0.89), + Coordinates(1.18, 0.68), + Coordinates(1.57, 0.7), + Coordinates(1.57, 0.87), + Coordinates(1.95, 0.85), + Coordinates(1.95, 0.72), + Coordinates(2.34, 0.79), + Coordinates(2.73, 0.79), + Coordinates(0.78, 0.26), + Coordinates(1.18, 0.36), + Coordinates(1.18, 0.16), + Coordinates(1.57, 0.18), + Coordinates(1.57, 0.35), + Coordinates(1.95, 0.32), + Coordinates(1.95, 0.2), + Coordinates(2.34, 0.26), + Coordinates(0.39, 6.02), + Coordinates(0.78, 6.02), + Coordinates(1.18, 6.12), + Coordinates(1.18, 5.92), + Coordinates(1.57, 5.94), + Coordinates(1.57, 6.1), + Coordinates(1.95, 6.08), + Coordinates(1.95, 5.96), + Coordinates(2.34, 6.02), + Coordinates(2.73, 6.02), + Coordinates(0.78, 5.5), + Coordinates(1.18, 5.6), + Coordinates(1.18, 5.4), + Coordinates(1.57, 5.41), + Coordinates(1.57, 5.58), + Coordinates(1.95, 5.56), + Coordinates(1.95, 5.44), + Coordinates(2.34, 5.5), + Coordinates(3.14, 4.97), + }; + + class MyServerCallbacks: public BLEServerCallbacks { + void onConnect(BLEServer* pServer) { + Serial.println("BLE: Device connected"); + } + void onDisconnect(BLEServer* pServer) { + Serial.println("BLE: Device disconnected"); + } + }; + + class ColorCharCallbacks: public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *pCharacteristic) { + Serial.println("[INFO]{BLE} Received a write on the color characteristic"); + uint8_t* pData = pCharacteristic->getData(); + singletoninstance->c_ble = (pData[2] << 16) | (pData[1] << 8) | pData[0]; + Serial.print("[DEBUG]{BLE}Qbead received"); + Serial.println(singletoninstance->c_ble, HEX); + } + }; + + class ThetaPhiCharCallbacks: public BLECharacteristicCallbacks { + void onWrite(BLECharacteristic *pCharacteristic) { + Serial.println("[INFO]{BLE} Received a write on the spherical coordinates characteristic"); + uint8_t* pData = pCharacteristic->getData(); + singletoninstance->t_ble = ((uint32_t)pData[0])*180/255; + singletoninstance->p_ble = ((uint32_t)pData[1])*360/255; + Serial.print("[DEBUG]{BLE} Received t="); + Serial.print(singletoninstance->t_ble); + Serial.print(" p="); + Serial.println(singletoninstance->p_ble); + } + }; + + void + begin() + { + Wire.begin(40, 39); + Wire.setClock(50000); // drop to 50kHz + singletoninstance = this; + Serial.begin(115200); + for (int waitCount = 0; waitCount < 50; waitCount++) + { + if (Serial) {break;} + delay(100); + } + + pixels.begin(); + T_led = millis(); + clear(); + setBrightness(10); + + Serial.println("[INFO] Booting... Qbead on XIAO ESP32 compiled on " __DATE__ " at " __TIME__); + + BLEDevice::init("qbead | " __DATE__ " " __TIME__); + // Bluefruit.begin(QB_MAX_PRPH_CONNECTION, 0); + // Bluefruit.setName("qbead | " __DATE__ " " __TIME__); + // Bluefruit.Periph.setConnectCallback(connect_callback); + bleserver = BLEDevice::createServer(); + bleserver->setCallbacks(new MyServerCallbacks()); + bleservice = bleserver->createService(QB_UUID_SERVICE); + // BLE Characteristic Bloch Sphere Visualizer color setup + + uint8_t zerobuffer2[] = {0 ,0}; + float zerobufferfloat[] = {0.0f, 0.0f, 0.0f}; + blecharcol = bleservice->createCharacteristic(QB_UUID_COL_CHAR, + BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE); + BLEDescriptor* pColDesc = new BLEDescriptor("2901"); + pColDesc->setValue("Color Characteristic"); + blecharcol->addDescriptor(pColDesc); + blecharcol->setCallbacks(new ColorCharCallbacks()); + blecharcol->setValue(zerobuffer20, 3); + + blecharsph = bleservice->createCharacteristic(QB_UUID_SPH_CHAR, + BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE); + BLEDescriptor* pSphDesc = new BLEDescriptor("2901"); + pSphDesc->setValue("Theta and Phi Characteristic"); + blecharsph->addDescriptor(pSphDesc); + blecharsph->setCallbacks(new ThetaPhiCharCallbacks()); + blecharsph->setValue(zerobuffer20, 2); + + blechargyr = bleservice->createCharacteristic(QB_UUID_GYR_CHAR, + BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY); + BLEDescriptor* pGyrDesc = new BLEDescriptor("2901"); + pGyrDesc->setValue("Gyroscope readout Characteristic"); + blechargyr->addDescriptor(pGyrDesc); + blechargyr->addDescriptor(new BLE2902()); + blechargyr->setValue(zerobuffer20, 3*sizeof(float)); + + startAccelerometer(); + + if (bleservice) { + Serial.println("starting service"); + bleservice->start(); + } else { + Serial.println("Service is null!"); + } + startBLEadv(); + + imuI2C.begin(Wire, QB_IMU_ADDR); + imuWE = ICM20948_WE(&Wire, QB_IMU_ADDR); + imuWE.setGyrRange(ICM20948_GYRO_RANGE_2000); + imuWE.setAccDLPF(ICM20948_DLPF_6); + imuWE.setAccRange(ICM20948_ACC_RANGE_8G); + } + + void startBLEadv(void) + { + bleadvertising = bleserver->getAdvertising(); + bleadvertising->addServiceUUID(QB_UUID_SERVICE); + Serial.println("[INFO]{BLE} Start advertising..."); + // Advertising packet + BLEAdvertisementData advertisementData; + advertisementData.setName("qbead | " __DATE__ " " __TIME__); + advertisementData.setFlags(6); // BLE_SIG_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE = 6 + + /* Start Advertising + * - Enable auto advertising if disconnected + * - Interval: fast mode = 20 ms, slow mode = 152.5 ms + * - Timeout for fast mode is 30 seconds + * - Start(timeout) with timeout = 0 will advertise forever (until connected) + * + * For recommended advertising interval + * https://developer.apple.com/library/content/qa/qa1931/_index.html + */ + bleadvertising->setAdvertisementData(advertisementData); + bleadvertising->setMinInterval(32); + bleadvertising->setMaxInterval(244); + + BLEDevice::startAdvertising(); + } + + void clear() { + pixels.clear(); + } + + void show() { + pixels.show(); + } + + void setBrightness(uint8_t b) { + pixels.setBrightness(b); + } + + void setLed(Coordinates coordinates, uint32_t color, int leds = 1) { + float theta = coordinates.theta() * 180 / PI; + float phi = coordinates.phi() * 180 / PI; + if (phi < 0) { + phi += 360; + } + setBloch_deg(theta, phi, color, leds); + } + + void showAxis() { + setLed(Coordinates(1, 0, 0), color(0, 0, 122)); + setLed(Coordinates(-1, 0, 0), color(0, 0, 122)); + setLed(Coordinates(0, 1, 0), color(0, 0, 122)); + setLed(Coordinates(0, -1, 0), color(0, 0, 122)); + setLed(Coordinates(0, 0, 1), color(0, 255, 0)); + setLed(Coordinates(0, 0, -1), color(255, 0, 0)); + } + + // in rads + float getDistToLed(float theta, float phi, int index) { + const Coordinates led = led_map_v1[index]; + const Coordinates reference(theta, phi); + return led.dist(reference.v); + } + + // Single bit is lit up on the Bloch sphere + void setBloch_deg(float theta, float phi, uint32_t c, int leds = 1) { + int index[leds]; + float dist[leds]; + for (int i = 0; i < leds; i++) { + index[i] = -1; + dist[i] = 1000; + } + for (int i = 0; i < QB_PIXEL_COUNT; i++) { + float d = getDistToLed(theta * PI / 180, phi * PI / 180, i); + for (int j = 0; j < leds; j++) { + if (d < dist[j]) { + for (int k = leds - 1; k > j; k--) { + index[k] = index[k - 1]; + dist[k] = dist[k - 1]; + } + index[j] = i; + dist[j] = d; + break; + } + } + } + for (int i = 0; i < leds; i++) { + if (index[i] != -1) { + uint8_t r = redch(c); + uint8_t g = greench(c); + uint8_t b = bluech(c); + float p2 = pow(200, -dist[i]); + pixels.setPixelColor(index[i], color(p2 * r, p2 * g, p2 * b)); + } + } + } + + void setBloch_deg_smooth(float theta, float phi, uint32_t c) { + setBloch_deg(theta, phi, c, 2); + } + + void animateTo(uint8_t gate, uint16_t animationLength = 2000) + { + if (gate == 0) + { + return; + } + if (gate == 9) + { + visualState.set(state.getCoordinates().v); + } + if (gate == 8) + { + state.collapse(); + visualState.set(state.getCoordinates().v); + } + float T_new = millis(); + float delta = T_new - T_freeze; + if (delta > animationLength) + { + frozen = false; + state.applyGateType(gate); + Serial.println("Animation finished"); + return; + } + float d = delta * PI / float(animationLength); + QuantumState from = state; + from.applyGateType(gate, d); + visualState.set(from.getCoordinates().v); + } + + bool detectShaking() + { + float totalAcceleration = gravityVector.norm(); + if (shakingState) + { + float newTime = millis(); + float shakingCounter = newTime - T_shaking; + if (shakingCounter < 300) + { + return false; + } + if (totalAcceleration > 3) + { + Serial.println("Randomizing"); + float randomTheta = (random(0, 1000)/1000.0f) * PI; + float randomPhi = (random(0, 1000)/500.0f) * PI; + state.setCoordinates(Coordinates(randomTheta, randomPhi)); + setLed(state.getCoordinates(), color(255, 0, 255)); + shakingState = false; + return true; + } + if (shakingCounter > 800) + { + shakingState = false; + } + return false; + } + if (totalAcceleration > 3) + { + Serial.print("Detected shaking turning on shakingState, acc length: "); + Serial.println(totalAcceleration); + shakingState = true; + T_shaking = millis(); + waitingForSecondTap = false; // reset double tap detection + } + return false; + } + + bool detectDoubleTap(float acc) + { + float currentTime = millis(); + + // If waiting too long for second tap, reset state + if (waitingForSecondTap && (currentTime - lastTapTime > TAP_THRESHOLD_TIME)) + { + waitingForSecondTap = false; + } + + // Check for tap condition + if (abs(acc) > TAP_THRESHOLD) + { + // Debounce: ensure enough time since last detected tap + if (currentTime - lastDebounceTime > DEBOUNCE_TIME) + { + lastDebounceTime = currentTime; + + if (waitingForSecondTap) + { + waitingForSecondTap = false; + return true; // Second tap detected within threshold time + } + else + { + // First tap detected + lastTapTime = currentTime; + waitingForSecondTap = true; + } + } else if (currentTime - lastDebounceTime > 30) { + // reset because it was a shake, not a tap + waitingForSecondTap = false; + } + } + return false; + } + + int checkMotion() + { + if (frozen) + { + return 0; + } + frozen = true; + T_freeze = micros(); + if (detectShaking()) + { + return 9; + } + if (shakingState) + { + frozen = false; + return 0; + } + // Handle double tap + float acc = (gravityVector(2) - oldGravityVector(2)) * 1000000 / dt; + Serial.print("acc: "); + Serial.println(acc); + if (detectDoubleTap(acc)) + { + Serial.println("Collapse detected"); + return 8; // collapse + } + float accX = (gravityVector(0) - oldGravityVector(0)) * 1000000 / dt; + float accY = (gravityVector(1) - oldGravityVector(1)) * 1000000 / dt; + if (detectDoubleTap(accX) || detectDoubleTap(accY)) + { + Serial.println("Hadamard detected"); + return 7; // Hadamard + } + // Handle rotating + for (int i = 0; i < 3; i++) + { + if (gyroVector[i] > GYRO_GATE_THRESHOLD) + { + return i + 1; // 1 = -x, 2 = -y, 3 = z + } + } + for (int i = 0; i < 3; i++) + { + if (gyroVector[i] < - GYRO_GATE_THRESHOLD) + { + return i + 4; // 4 = x, 5 = y, 6 = -z + } + } + frozen = false; + return 0; + } + + void writeToBLE(BLECharacteristic* destination, Vector3d vector) { + float buffer[] = {(float)vector(0), (float)vector(1), (float)vector(2)}; + if (destination) + { + destination->setValue((uint8_t*)buffer, sizeof(buffer)); + destination->notify(); + } else + { + Serial.println("destination is null"); + } + } + + Vector3d getVectorFromBuffer(float *buffer) { + // calibration of imu because imu is not aligned with bloch sphere + float rx = (1 - 2 * QB_SX) * buffer[QB_IX]; + float ry = (1 - 2 * QB_SY) * buffer[QB_IY]; + float rz = (1 - 2 * QB_SZ) * buffer[QB_IZ]; + return Vector3d(rx, ry, rz); + } + + void readIMU(bool print=true) { + while (!imuI2C.dataReady()) { + delay(1); // 1-2 ms delay is fine + } + + imuI2C.getAGMT(); + rbuffer[0] = imuI2C.accX() / 1000.0f; // convert to g + rbuffer[1] = imuI2C.accY() / 1000.0f; + rbuffer[2] = imuI2C.accZ() / 1000.0f; + rgyrobuffer[0] = imuI2C.gyrX(); + rgyrobuffer[1] = imuI2C.gyrY(); + rgyrobuffer[2] = imuI2C.gyrZ(); + + float T_new = micros(); + dt = T_new - T_imu; + T_imu = T_new; + + Vector3d newGyro = getVectorFromBuffer(rgyrobuffer) * PI / 180; + float d = min(dt / float(T_GYRO), 1.0f); + gyroVector = d * newGyro + (1 - d) * gyroVector; // low pass filter + + Vector3d newGravity = getVectorFromBuffer(rbuffer); + d = min(dt / float(T_ACC), 1.0f); + oldGravityVector = gravityVector; + gravityVector = d * newGravity + (1 - d) * gravityVector; + + if (print) { + Serial.print(gravityVector(0)); + Serial.print("\t"); + Serial.print(gravityVector(1)); + Serial.print("\t"); + Serial.print(gravityVector(2)); + Serial.print("\t-1\t1\t"); + Serial.print(gyroVector(0)); + Serial.print("\t"); + Serial.print(gyroVector(1)); + Serial.print("\t"); + Serial.println(gyroVector(2)); + } + + if (blecharacc) { + writeToBLE(blecharacc, gravityVector); + } + if (blechargyr) { + writeToBLE(blechargyr, gyroVector); + } + } +}; // end class + +Qbead *Qbead::singletoninstance = nullptr; + +} // end namespace + +#endif // QBEAD_H \ No newline at end of file