Brody's Builds
← All projects
Self-Balancing Triangle with a Phone Remote
ESP32HardAn afternoon

Self-Balancing Triangle with a Phone Remote

I opened up a self-balancing triangle toy, backed up its factory program and wrote new code for it, so a phone can make it lean, fall over, get back up and run light shows.

Board: ESP32-WROOM-32 (inside the toy)

What you need

What it is

The NikolaToy triangle is a rounded triangle (a Reuleaux triangle) that balances on its edge all by itself, using a heavy metal wheel spinning inside it. It came with no app and no way to control it. I wanted to drive it from my phone, so I:

  1. found the hidden programming holes on its circuit board,
  2. made a full backup of the program it came with,
  3. worked out how this toy is different from the open-source design it's based on,
  4. and wrote new firmware with its own Wi-Fi hotspot, plus an Android remote app.

It balances better than before and gets up faster than the factory program did.

How it balances

  • A tilt sensor (an MPU6050, the same chip as the GY-521 module) measures which way the triangle is leaning, hundreds of times a second. A Kalman filter smooths out the shaky readings.
  • The reaction wheel: when the triangle starts to fall one way, the motor spins the wheel the other way. Spinning the wheel pushes back on the triangle (like recoil), which tips it back up.
  • The controller (called LQR) mixes three things to decide how hard to push: how far it's leaning, how fast it's falling, and how fast the wheel is already spinning.
  • Getting up: to stand up from lying on its side, it spins the wheel fast and then brakes hard, which swings the triangle up onto its edge.

Tip: The big discovery: NikolaToy swapped the wheel's position sensor for a different kind (a 14-bit SSI encoder on pins 5, 22 and 23) than the open-source design uses. The open-source code couldn't find the motor at all until I read the factory program's instructions and found the change. This code has the fix built in.

What the phone remote does

  • Tilt your phone to make the triangle lean, or use the lean buttons.
  • Tricks: fall over left or right, get back up, roll onto the next corner.
  • Shows: wiggle, fall-and-rise, roll, or everything in a row.
  • Lights: 11 effects, brightness, and any colour.
  • Live info: battery voltage, angle, wheel speed.
  • Fine-tuning sliders for how far it leans and how hard it pushes.
The Triangle Remote app
The Triangle Remote app

Build it

1Open it up

Take out the screws at the corners and lift off the white front frame. Under it is the circuit board. The USB-C port is for charging only, so it can't be used to program the toy. The three holes labelled RXD, TXD, GND above the ESP32 chip are the programming port.

The circuit board: programming holes, BOOT and RESET buttons, power switch
The circuit board: programming holes, BOOT and RESET buttons, power switch

2Connect the adapter

Three wires, and two of them cross over: the adapter's TXD goes to the triangle's RXD, and its RXD goes to the triangle's TXD. GND goes to GND. Leave the adapter's 3V3 and +5V pins empty: the triangle runs on its own battery, so switch it on.

Adapter wiring: TXD to RXD, RXD to TXD, GND to GND
Adapter wiring: TXD to RXD, RXD to TXD, GND to GND

Tip: The holes are bare, so push the wire pins in, tilt them slightly so they touch the sides, and tape them down. If the Serial Monitor shows nothing, a pin has probably slipped.

3Back up the factory program first

Do this before uploading anything, so you can always put the toy back exactly how it came.

  1. Install Python, then in a terminal run pip install esptool.
  2. Put the triangle in download mode: hold BOOT, tap RESET, let go of BOOT.
  3. Run this (change COM4 to your adapter's port):
python -m esptool --port COM4 --baud 460800 --before no-reset read-flash 0 ALL factory-firmware.bin

It takes about 2 minutes. Keep factory-firmware.bin somewhere safe. To restore it later (download mode first):

python -m esptool --port COM4 --baud 460800 --before no-reset write-flash 0 factory-firmware.bin

4Set up the Arduino app

  1. Boards Manager: search esp32 (by Espressif) and install version 2.0.11. Newer 3.x versions changed things this code uses.
  2. Library Manager: install Simple FOC version 2.3.2 and Adafruit NeoPixel.
  3. Board: ESP32 Dev Module, Flash Mode DIO, Partition Scheme Default.

5Upload

Open TriangleRemote.ino, click Upload, and when the bottom says Connecting..., do the BOOT + RESET trick from step 3. When it's done, press RESET to start the new program. The toy's own calibration is kept, because the new code reads the same saved settings.

6Install the remote app

Download the app below and install it on an Android phone (you'll need to allow installing apps from your browser). Open it and it joins the triangle's hotspot, Nikola-Triangle-XXXX, by itself.

Warning: The hotspot's password is triangle123, set in AP_PASSWORD near the top of the code. The app uses the same one, so if you change it, the app needs changing too. Type WIFI 0 to turn the hotspot off when you're not using it.

Commands

The app sends these over Wi-Fi (UDP port 2333). You can also type them in the Serial Monitor at 115200 baud.

CommandWhat it does
LEAN fLean (f from -1 to 1)
PUSH L / PUSH RLean hard one way for a moment
FALL L / FALL RTip over and lie down
RISEGet back up and balance
ROLL L / ROLL RRoll onto the next corner (experimental)
SHOW n1 wiggle, 2 fall and rise, 3 roll, 4 everything, 0 stop
STOP / GOMotor off / back to balancing
RGB n, BRI n, COL r g bLight effect, brightness, colour
STATStatus line

If it doesn't work

  • Nothing in the Serial Monitor: a wire has slipped out of its hole. Push it back in and press RESET.
  • "MOT: Failed to notice movement": the motor test failed. Charge the battery (it needs about 10 V or more) and make sure you're using this code, which reads NikolaToy's wheel sensor.
  • Upload says "Wrong boot mode": do the BOOT + RESET trick again while it says Connecting.
  • The wheel spins up while it's lying flat: it's trying to balance. Send STOP, or stand it on its edge.
  • "Get up" struggles right after a fall: version 1.1 fixes this by braking the wheel to a stop before swinging up.
  • The lean looks small: that's physics. A reaction wheel can only push for a moment, so it can sway and dip but not hold a big lean. Raise the lean sliders in the app for more.

Credits

Based on the open-source self-balancing Reuleaux triangle v2 firmware (gitee.com/Blue-F/foc, GPL-3.0), which is what the toy shipped with. Uses the Simple FOC motor library and the Kalman filter by TKJ Electronics. This version is shared under GPL-3.0 too.

The code

Five files that go in one folder called TriangleRemote: download the .zip and open TriangleRemote.ino. Board: ESP32 Dev Module. This code is GPL-3.0, like the open-source project it's based on (the licence is in the zip).

Download all the code (.zip)
TriangleRemote.ino · 1001 lines Download
/*  TriangleRemote: phone-controlled firmware for the NikolaToy self-balancing  Reuleaux triangle (ESP32-WROOM-32, EG2133 motor driver, AS5600 wheel sensor,  MPU6050 tilt sensor, 21 WS2812 lights).  Based on the open-source "self-balancing Reuleaux triangle" v2 firmware  (gitee.com/Blue-F/foc, GPL-3.0), which is what the toy shipped with. The  balancing (Kalman filter, LQR, swing-up, auto-trim) works the same way and  reads the same saved settings, so the toy's own calibration carries over.  New: an always-on Wi-Fi hotspot with UDP commands for the phone app,  tilt-to-lean, lean/fall/rise/roll tricks, shows, and more light effects.  Pins (read out of the factory firmware): motor phases 32/33/25, wheel  encoder SSI clock 5 / select 22 / data 23 (NikolaToy's change; the  open-source board had an AS5600 on 23/5), MPU6050 SDA 19 / SCL 18, lights 16,  status LED 4, battery sense 34 (62k/10k divider), touch pads T2 (GPIO2),  T3 (GPIO15), T4 (GPIO13).  Commands: UDP port 2333 on the hotspot, or Serial at 115200. One per line;  several in one packet can be split with ';' or new lines.    PING                  reply "PONG ..." and send status packets to the sender    LEAN f                tilt mode: lean f x leanMax degrees (f = -1..1); stops 0.4 s after the last one    PUSH L|R              lean hard one way for a moment    FALL L|R              tip over that way and lie down    RISE                  get back up and balance    ROLL L|R              roll over onto the next corner (experimental)    SHOW n                1 wiggle, 2 fall and rise, 3 roll, 4 everything, 0 stop    STOP / GO             motor off / back to balancing    RGB n | BRI n | COL r g b     light effect (0-10), brightness (0-255), colour for effect 8    SET key value         tune and save: leanMax pushDeg pushMs kickV kickMs rollV rollMs dir    STAT                  status line now    WIFI 0|1              hotspot off / on  The original tuning commands still work too: TA SV SA VP1 VI1 VP2 VI2 K31-K43  MOTOR TVQ TVV VV VQ TV START (START streams the old-style data to 192.168.4.2).*/#include <SimpleFOC.h>#include <WiFi.h>#include <AsyncUDP.h>#include "EEPROM.h"#include "Kalman.h"  // Source: https://github.com/TKJElectronics/KalmanFilter#include "lights.h"#define FW_VERSION "1.1"// Declared up here because the Arduino builder puts its function prototypes// before the first function, and some of them use this type.enum Mode { M_BALANCE, M_REST, M_KICK, M_OFF, M_BRAKE };/* ---- ESP32 pins ---- */#define ACTIVE_PIN 4              // status light#define BAT_VOLTAGE_SENSE_PIN 34  // battery voltage (ADC1)const double R1_VOLTAGE = 62000;const double R2_VOLTAGE = 10000;const double min_voltage = 9.5;   // below this the motor stops (3 cells)double bat_voltage;unsigned long voltage_last_time;bool battery_low = 0;/* ---- tilt sensor ---- */Kalman kalmanZ;#define gyroZ_OFF -0.19double accX, accY, accZ;double gyroX, gyroY, gyroZ;double gyroZangle, compAngleZ, kalAngleZ;uint32_t timer;uint8_t i2cData[14];float pendulum_angle = 0;  // how far from the balance point (degrees)float gyroZrate = 0;/* ---- wheel position sensor ----   NikolaToy's board doesn't use the open-source design's AS5600. Its factory   firmware reads a 14-bit magnetic encoder over 3 wires (SSI): clock GPIO5,   select GPIO22, data GPIO23. This reads it exactly the same way. */#define ENC_CLK 5#define ENC_CS 22#define ENC_DO 23void initEncoder() {  pinMode(ENC_CLK, OUTPUT);  pinMode(ENC_CS, OUTPUT);  pinMode(ENC_DO, INPUT);  digitalWrite(ENC_CS, HIGH);  digitalWrite(ENC_CLK, HIGH);}float readEncoder() {  digitalWrite(ENC_CS, LOW);  digitalWrite(ENC_CLK, LOW);  uint16_t raw = 0;  for (int i = 0; i < 16; i++) {    digitalWrite(ENC_CLK, HIGH);    delayMicroseconds(1);    raw = (raw << 1) | digitalRead(ENC_DO);    digitalWrite(ENC_CLK, LOW);    delayMicroseconds(1);  }  digitalWrite(ENC_CS, HIGH);  digitalWrite(ENC_CLK, HIGH);  raw >>= 2;                       // 14-bit angle  return raw * (_2PI / 16384.0f);}/* ---- motor ---- */GenericSensor sensor = GenericSensor(readEncoder, initEncoder);BLDCMotor motor = BLDCMotor(7);BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25);// LQR gains as NikolaToy tuned them in the factory firmware// (open-source defaults were K3 8.4/2.1/2.1, K4 2.4/1.5/1.42).// K3 while catching, K4 once settled.float LQR_K3_1 = 12.0, LQR_K3_2 = 1.9, LQR_K3_3 = 1.8;float LQR_K4_1 = 4.4, LQR_K4_2 = 1.5, LQR_K4_3 = 1.42;// Defaults match the factory firmware; the saved values (EEPROM 0-24) win.float target_velocity = 0;float target_angle = 89.5;     // balance angle, saved at EEPROM 0 (the toy's own calibration)float target_voltage = 0;float swing_up_voltage = 2.3;  // push used to rock itself back upfloat swing_up_angle = 18;     // closer than this to balance -> LQR takes overfloat v_i_1 = 15, v_p_1 = 0.25; // wheel speed loop while catchingfloat v_i_2 = 10, v_p_2 = 0.1;  // wheel speed loop once settledbool stable = 0;uint32_t last_unstable_time, last_stable_time;int test_flag = 0;             // original test modes: 1 = fixed voltage, 2 = fixed speedbool Motor_enable_flag = 0;    // original MOTOR command togglebool motorOk = false;          // the motor's start-up self-check passedbool spinTest = false;         // SPIN: turn the wheel without the sensor (driver test)float spinVelocity = 0;/* ---- what the triangle is doing ---- */Mode mode = M_BALANCE;Mode afterKick = M_REST;float kickVolts = 0;uint32_t kickUntil = 0;// Getting up works badly while the wheel is still coasting from a fall (the// rocking pushes come out lopsided), so RISE first brakes the wheel to a stop.const float BRAKE_VOLTS = 4;     // gentle, so braking doesn't throw it aboutconst float CALM_SPEED = 6;      // rad/s: slow enough to start rocking upuint32_t brakeStart = 0, calmSince = 0;const char *modeName(Mode m) {  switch (m) {    case M_BALANCE:    case M_BRAKE: return "BAL";  // the app shows both as "Getting up"    case M_REST: return "REST";    case M_KICK: return "KICK";    default: return "OFF";  }}/* ---- tunables (saved from EEPROM 100) ---- */struct Tune {  float leanMax = 3.0;  // degrees at full phone tilt  float pushDeg = 4.0;  // degrees for PUSH  float pushMs = 450;   // how long a PUSH lasts  float kickV = 6.0;    // volts for the FALL kick  float kickMs = 180;  float rollV = 12.0;   // volts for the ROLL kick  float rollMs = 260;  float dir = 1;        // flips left/right if they come out backwards} tune;const int TUNE_ADDR = 100;const uint8_t TUNE_MAGIC = 0xA7;/* ---- leaning ---- */float tiltLean = 0;         // from the phone's tilt (degrees)uint32_t tiltUntil = 0;float pushLean = 0;uint32_t pushUntil = 0;float leanNow = 0;          // smoothed lean actually useduint32_t leanFreezeUntil = 0;const float LEAN_RATE = 25; // degrees per second/* ---- Wi-Fi ---- */const char *AP_PASSWORD = "triangle123";char apName[32];bool wifi_on = false;AsyncUDP udp;const uint16_t UDP_PORT = 2333;struct Msg {  char text[64];  uint32_t ip;  uint16_t port;};QueueHandle_t inbox;uint32_t clientIp = 0;uint16_t clientPort = 0;uint32_t clientSeen = 0;uint32_t lastStatus = 0;bool legacyStream = false;  // original START command/* ---- touch pads (same behaviour as the original) ---- */const int threshold_top = 30;  // same as the factory firmware (the open-source one used 40)const int single_count = 10;   // x10 ms = a tapconst int long_count = 80;     // x10 ms = a long pressunsigned long touch_last_time;int touch_count[3] = {0, 0, 0};int touch_touched[3] = {0, 0, 0};/* ---- shows ---- */enum Op : uint8_t { O_END, O_PUSH, O_WAIT, O_FALL, O_RISE, O_ROLL, O_WAITUP, O_LIGHT };struct Step {  Op op;  int8_t arg;      // -1 left / +1 right, or light effect  uint16_t ms;};const Step SHOW_WIGGLE[] = {  {O_LIGHT, 10, 0},  {O_PUSH, -1, 0}, {O_WAIT, 0, 750}, {O_PUSH, 1, 0}, {O_WAIT, 0, 750},  {O_PUSH, -1, 0}, {O_WAIT, 0, 750}, {O_PUSH, 1, 0}, {O_WAIT, 0, 750},  {O_PUSH, -1, 0}, {O_WAIT, 0, 750}, {O_PUSH, 1, 0}, {O_WAIT, 0, 900},  {O_LIGHT, -1, 0}, {O_END, 0, 0}};const Step SHOW_FALL[] = {  {O_LIGHT, 11, 0}, {O_FALL, -1, 0}, {O_WAIT, 0, 2500},  {O_LIGHT, 5, 0}, {O_RISE, 0, 0}, {O_WAITUP, 0, 12000}, {O_WAIT, 0, 1500},  {O_LIGHT, 11, 0}, {O_FALL, 1, 0}, {O_WAIT, 0, 2500},  {O_LIGHT, 5, 0}, {O_RISE, 0, 0}, {O_WAITUP, 0, 12000},  {O_LIGHT, 12, 0}, {O_WAIT, 0, 900}, {O_LIGHT, -1, 0}, {O_END, 0, 0}};const Step SHOW_ROLL[] = {  {O_LIGHT, 9, 0}, {O_ROLL, -1, 0}, {O_WAITUP, 0, 12000}, {O_WAIT, 0, 1500},  {O_ROLL, 1, 0}, {O_WAITUP, 0, 12000},  {O_LIGHT, 12, 0}, {O_WAIT, 0, 900}, {O_LIGHT, -1, 0}, {O_END, 0, 0}};const Step SHOW_ALL[] = {  {O_LIGHT, 10, 0},  {O_PUSH, -1, 0}, {O_WAIT, 0, 750}, {O_PUSH, 1, 0}, {O_WAIT, 0, 750},  {O_PUSH, -1, 0}, {O_WAIT, 0, 750}, {O_PUSH, 1, 0}, {O_WAIT, 0, 1200},  {O_LIGHT, 11, 0}, {O_FALL, -1, 0}, {O_WAIT, 0, 2500},  {O_LIGHT, 5, 0}, {O_RISE, 0, 0}, {O_WAITUP, 0, 12000}, {O_WAIT, 0, 1500},  {O_LIGHT, 9, 0}, {O_ROLL, 1, 0}, {O_WAITUP, 0, 12000}, {O_WAIT, 0, 1500},  {O_LIGHT, 7, 0}, {O_PUSH, 1, 0}, {O_WAIT, 0, 750}, {O_PUSH, -1, 0}, {O_WAIT, 0, 900},  {O_LIGHT, 12, 0}, {O_WAIT, 0, 1200}, {O_LIGHT, -1, 0}, {O_END, 0, 0}};const Step *show = nullptr;int showNumber = 0;int showStep = 0;uint32_t stepStart = 0;bool stepStarted = false;uint32_t uprightSince = 0;/* ======================================================================== */void setup() {  Serial.begin(115200);  pinMode(ACTIVE_PIN, OUTPUT);  digitalWrite(ACTIVE_PIN, LOW);  Serial.println();  Serial.println("TriangleRemote " FW_VERSION);  if (!EEPROM.begin(1000)) {    Serial.println("Failed to initialise EEPROM");    delay(1000);    ESP.restart();  }  loadSettings();  strip.begin();  strip.show();  strip.setBrightness(brightness);  colorWipe_delay(strip.Color(255, 106, 106), 30);  colorWipe_delay(strip.Color(0, 255, 255), 30);  colorWipe_delay(strip.Color(148, 0, 211), 30);  // tilt sensor (MPU6050)  Wire.begin(19, 18, 1000000);  // the factory firmware runs it at the ESP32's 1 MHz maximum  i2cData[0] = 7;     // 1 kHz sample rate  i2cData[1] = 0x00;  // 260 Hz acc filter, 256 Hz gyro filter  i2cData[2] = 0x00;  // gyro +-250 deg/s  i2cData[3] = 0x00;  // acc +-2 g  while (i2cWrite(0x19, i2cData, 4, false));  while (i2cWrite(0x6B, 0x01, true));  while (i2cRead(0x75, i2cData, 1));  if (i2cData[0] != 0x68) {    Serial.print(F("Error reading sensor"));    while (1);  }  delay(100);  while (i2cRead(0x3B, i2cData, 6));  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);  double pitch = acc2rotation(accX, accY);  kalmanZ.setAngle(pitch);  gyroZangle = pitch;  timer = micros();  Serial.println("kalman mpu6050 init");  // wheel motor  sensor.init();  motor.linkSensor(&sensor);  driver.voltage_power_supply = 12;  driver.init();  motor.linkDriver(&driver);  motor.foc_modulation = FOCModulationType::SpaceVectorPWM;  motor.controller = MotionControlType::torque;  motor.PID_velocity.P = v_p_1;  motor.PID_velocity.I = v_i_1;  motor.voltage_limit = 12;  motor.LPF_velocity.Tf = 0.01;  motor.velocity_limit = 40;  motor.useMonitoring(Serial);  motor.init();  startMotor(3);  inbox = xQueueCreate(16, sizeof(Msg));  wifiStart();  digitalWrite(ACTIVE_PIN, HIGH);  Serial.printf("Ready. Wi-Fi \"%s\" password \"%s\", UDP port %u\r\n", apName, AP_PASSWORD, UDP_PORT);}void loop() {  motor.loopFOC();  uint32_t now = millis();  handleMessages();  readSerial();  if (now - voltage_last_time >= 1000) {    voltage_last_time = now;    voltage_detection();  }  if (now - touch_last_time >= 10) {    touch_last_time = now;    readTouch();  }  if (battery_low) {    digitalWrite(ACTIVE_PIN, (now % 500) < 250);  }  lightsUpdate(motor.shaft_velocity);  runShow(now);  // ---- read the tilt sensor ----  while (i2cRead(0x3B, i2cData, 14));  accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);  accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);  accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);  gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);  gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);  gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);  double dt = (double)(micros() - timer) / 1000000;  timer = micros();  double pitch = acc2rotation(accX, accY);  gyroZrate = gyroZ / 131.0;  kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt);  gyroZangle += (gyroZrate + gyroZ_OFF) * dt;  compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch;  if (gyroZangle < -180 || gyroZangle > 180) gyroZangle = kalAngleZ;  updateLean(dt);  pendulum_angle = constrainAngle(fmod(kalAngleZ, 120) - (target_angle + leanNow));  // ---- drive the wheel ----  if (mode == M_KICK && now >= kickUntil) {    mode = afterKick;    if (mode == M_BALANCE) last_unstable_time = now;  }  if (spinTest) {    motor.move(spinVelocity);  } else if (!motorOk) {    // the motor's self-check failed; leave it off  } else if (test_flag == 1) {    motor.move(target_voltage);  } else if (test_flag == 2) {    motor.move(target_velocity);  } else if (mode == M_OFF) {    // motor is disabled; nothing to do  } else if (mode == M_REST) {    motor.controller = MotionControlType::torque;    motor.move(0);  } else if (mode == M_KICK) {    motor.controller = MotionControlType::torque;    motor.move(kickVolts);  } else if (mode == M_BRAKE) {    motor.controller = MotionControlType::velocity;    motor.move(0);    if (abs(motor.shaft_velocity) < CALM_SPEED) {      if (!calmSince) calmSince = now;    } else {      calmSince = 0;    }    if ((calmSince && now - calmSince > 250) || now - brakeStart > 3000) {      endBrake();      mode = M_BALANCE;      stable = 0;      last_unstable_time = now;    }  } else {    if (abs(pendulum_angle) < swing_up_angle) {  // close to balance: LQR holds it      target_velocity = controllerLQR(pendulum_angle, gyroZrate, motor.shaft_velocity);      if (abs(target_velocity) > 120) target_velocity = _sign(target_velocity) * 120;      motor.controller = MotionControlType::velocity;      motor.move(target_velocity);    } else {                                    // too far over: rock back up      motor.controller = MotionControlType::torque;      target_voltage = -_sign(gyroZrate) * swing_up_voltage;      motor.move(target_voltage);    }  }  if (abs(pendulum_angle) < 3 && mode == M_BALANCE) {    if (!uprightSince) uprightSince = now;  } else {    uprightSince = 0;  }  sendStatus(now);}/* ---- motor start-up ---- */// Runs the motor's self-check (it turns the wheel a little to find its// position). Tries again if the wheel didn't move, e.g. because it was held.void startMotor(int attempts) {  for (int a = 1; a <= attempts; a++) {    sensor.update();    Serial.printf("Wheel sensor reads %.1f deg, battery %.2f V\r\n", sensor.getAngle() * 57.2958,                  return_voltage_value(BAT_VOLTAGE_SENSE_PIN));    motor.enable();    motorOk = motor.initFOC();    if (motorOk) {      Serial.println(F("Motor ready."));      return;    }    Serial.printf("Motor self-check failed (try %d of %d)\r\n", a, attempts);    delay(800);  }  motor.disable();  Serial.println(F("MOTOR NOT READY - send INIT to try again"));}/* ---- balancing (same maths as the original) ---- */// MPU6050 acceleration -> angledouble acc2rotation(double x, double y) {  double tmp_kalAngleZ = (atan(x / y) / 1.570796 * 90);  if (y < 0) return (tmp_kalAngleZ + 180);  if (x < 0) {    if (!isnan(kalAngleZ) && (tmp_kalAngleZ + 360 - kalAngleZ) > 100) return tmp_kalAngleZ;    return (tmp_kalAngleZ + 360);  }  return tmp_kalAngleZ;}// keep the angle error within -60..60 (the triangle has three corners)float constrainAngle(float x) {  if (x < 0) {    float a = 120 + x;    if (a < abs(x)) return a;  }  return x;}float controllerLQR(float p_angle, float p_vel, float m_vel) {  uint32_t now = millis();  if (now < leanFreezeUntil) {    // While leaning on purpose, stay in "catching" mode and don't re-learn    // the balance point, so it comes back to the same place afterwards.    last_unstable_time = now;    stable = 0;  }  if (abs(p_angle) > 5) {    last_unstable_time = now;    if (stable) {      target_angle = EEPROM.readFloat(0);      stable = 0;    }  }  if ((now - last_unstable_time) > 1000 && !stable) {    target_angle = target_angle + p_angle;   // settled: learn where balance really is    stable = 1;  }  if ((now - last_stable_time) > 2500 && stable) {    if (abs(target_velocity) > 5) {          // wheel creeping: nudge the balance point      last_stable_time = now;      target_angle -= _sign(target_velocity) * 0.2;    }  }  float u;  if (!stable) {    motor.PID_velocity.P = v_p_1;    motor.PID_velocity.I = v_i_1;    u = LQR_K3_1 * p_angle + LQR_K3_2 * p_vel + LQR_K3_3 * m_vel;  } else {    motor.PID_velocity.P = v_p_2;    motor.PID_velocity.I = v_i_2;    u = LQR_K4_1 * p_angle + LQR_K4_2 * p_vel + LQR_K4_3 * m_vel;  }  return u;}/* ---- leaning ---- */void updateLean(double dt) {  uint32_t now = millis();  float want = 0;  if (now < pushUntil) want = pushLean;  else if (now < tiltUntil) want = tiltLean;  if (mode != M_BALANCE) want = 0;  float step = LEAN_RATE * dt;  if (want > leanNow + step) leanNow += step;  else if (want < leanNow - step) leanNow -= step;  else leanNow = want;  if (abs(leanNow) > 0.05 || want != 0) leanFreezeUntil = now + 1500;}/* ---- tricks ---- */float sideSign(char side) {  return (side == 'L' || side == 'l' || side == '-') ? -tune.dir : tune.dir;}void doPush(float s) {  pushLean = s * tune.pushDeg;  pushUntil = millis() + (uint32_t)tune.pushMs;}void doKick(float volts, float ms, Mode then) {  if (battery_low) return;  if (mode == M_OFF) motor.enable();  if (mode == M_BRAKE) endBrake();  kickVolts = volts;  kickUntil = millis() + (uint32_t)ms;  afterKick = then;  mode = M_KICK;  stable = 0;}void doFall(float s) { doKick(s * tune.kickV, tune.kickMs, M_REST); }void doRoll(float s) { doKick(s * tune.rollV, tune.rollMs, M_BALANCE); }void doRise() {  if (battery_low) return;  if (mode == M_OFF) motor.enable();  stable = 0;  last_unstable_time = millis();  if (abs(motor.shaft_velocity) > CALM_SPEED) {    mode = M_BRAKE;   // stop the wheel first, then rock up    brakeStart = millis();    calmSince = 0;    motor.PID_velocity.P = v_p_1;    motor.PID_velocity.I = v_i_1;    motor.PID_velocity.limit = BRAKE_VOLTS;    motor.PID_velocity.reset();  } else {    mode = M_BALANCE;  }}void endBrake() {  motor.PID_velocity.limit = motor.voltage_limit;  motor.PID_velocity.reset();}void doStop() {  stopShow();  if (mode == M_BRAKE) endBrake();  mode = M_OFF;  test_flag = 0;  motor.disable();}void startShow(int n) {  stopShow();  if (n == 1) show = SHOW_WIGGLE;  else if (n == 2) show = SHOW_FALL;  else if (n == 3) show = SHOW_ROLL;  else if (n == 4) show = SHOW_ALL;  else return;  if (battery_low) { show = nullptr; return; }  if (mode == M_OFF) doRise();  showNumber = n;  showStep = 0;  stepStarted = false;}void stopShow() {  show = nullptr;  showNumber = 0;  overlay = -1;  pushUntil = 0;}void runShow(uint32_t now) {  if (!show) return;  const Step &s = show[showStep];  if (!stepStarted) {    stepStarted = true;    stepStart = now;    switch (s.op) {      case O_END: stopShow(); return;      case O_PUSH: doPush(s.arg * tune.dir); break;      case O_FALL: doFall(s.arg * tune.dir); break;      case O_RISE: doRise(); break;      case O_ROLL: doRoll(s.arg * tune.dir); break;      case O_LIGHT: overlay = s.arg; break;      default: break;    }  }  bool done = true;  if (s.op == O_WAIT) done = now - stepStart >= s.ms;  else if (s.op == O_WAITUP) done = (uprightSince && now - uprightSince > 800) || now - stepStart >= s.ms;  if (done) {    showStep++;    stepStarted = false;  }}/* ---- commands ---- */// Original tuning commands (prefix + number), checked in the original's order.bool legacyCommand(const char *c) {  struct L { const char *id; float *v; int addr; };  static const L floats[] = {    {"TA", &target_angle, 0}, {"SV", &swing_up_voltage, 4}, {"SA", &swing_up_angle, 8},    {"VP1", &v_p_1, 12}, {"VI1", &v_i_1, 16}, {"VP2", &v_p_2, 20}, {"VI2", &v_i_2, 24},    {"K31", &LQR_K3_1, -1}, {"K32", &LQR_K3_2, -1}, {"K33", &LQR_K3_3, -1},    {"K41", &LQR_K4_1, -1}, {"K42", &LQR_K4_2, -1}, {"K43", &LQR_K4_3, -1}};  if (!strcmp(c, "START")) { legacyStream = !legacyStream; return true; }  if (!strcmp(c, "MOTOR")) {    if (mode == M_BRAKE) endBrake();    if (Motor_enable_flag) { motor.enable(); mode = M_BALANCE; }    else { motor.disable(); mode = M_OFF; }    Motor_enable_flag = !Motor_enable_flag;    return true;  }  if (!strncmp(c, "TVQ", 3)) {    if (test_flag == 1) test_flag = 0;    else { motor.controller = MotionControlType::torque; test_flag = 1; }    return true;  }  if (!strncmp(c, "TVV", 3)) {    if (test_flag == 2) test_flag = 0;    else { motor.controller = MotionControlType::velocity; test_flag = 2; }    return true;  }  if (!strncmp(c, "VV", 2)) { target_velocity = atof(c + 2); return true; }  if (!strncmp(c, "VQ", 2)) { target_voltage = atof(c + 2); return true; }  for (const L &l : floats) {    size_t n = strlen(l.id);    if (!strncmp(c, l.id, n) && (isdigit(c[n]) || c[n] == '-' || c[n] == '.')) {      *l.v = atof(c + n);      if (l.addr >= 0) { EEPROM.writeFloat(l.addr, *l.v); EEPROM.commit(); }      return true;    }  }  if (!strncmp(c, "TV", 2)) { target_velocity = atof(c + 2); return true; }  return false;}bool setTune(const char *key, float v) {  struct K { const char *name; float *p; float lo, hi; };  K keys[] = {    {"leanMax", &tune.leanMax, 0, 10}, {"pushDeg", &tune.pushDeg, 0, 12},    {"pushMs", &tune.pushMs, 50, 3000}, {"kickV", &tune.kickV, 0, 12},    {"kickMs", &tune.kickMs, 20, 1500}, {"rollV", &tune.rollV, 0, 12},    {"rollMs", &tune.rollMs, 20, 1500}, {"dir", &tune.dir, -1, 1}};  for (K &k : keys) {    if (!strcasecmp(key, k.name)) {      *k.p = constrain(v, k.lo, k.hi);      if (k.p == &tune.dir) tune.dir = tune.dir < 0 ? -1 : 1;      saveTune();      return true;    }  }  return false;}// Runs one command; the reply (if any) goes into `reply`.void runCommand(char *c, String &reply) {  while (*c == ' ') c++;  if (!*c) return;  char word[12] = {0};  int i = 0;  while (c[i] && c[i] != ' ' && i < 11) { word[i] = toupper(c[i]); i++; }  const char *arg = c + i;  while (*arg == ' ') arg++;  if (!strcmp(word, "PING")) {    reply = String("PONG ") + apName + " fw=" FW_VERSION;  } else if (!strcmp(word, "STAT")) {    reply = statusLine();  } else if (!strcmp(word, "LEAN")) {    tiltLean = constrain(atof(arg), -1.0f, 1.0f) * tune.leanMax * tune.dir;    tiltUntil = millis() + 400;  } else if (!strcmp(word, "PUSH")) {    doPush(sideSign(arg[0]));  } else if (!strcmp(word, "FALL")) {    stopShow();    doFall(sideSign(arg[0]));  } else if (!strcmp(word, "RISE")) {    stopShow();    doRise();  } else if (!strcmp(word, "ROLL")) {    stopShow();    doRoll(sideSign(arg[0]));  } else if (!strcmp(word, "SHOW")) {    int n = atoi(arg);    if (n) startShow(n);    else stopShow();  } else if (!strcmp(word, "STOP")) {    doStop();  } else if (!strcmp(word, "GO")) {    test_flag = 0;    doRise();  } else if (!strcmp(word, "RGB")) {    rgb_flag = constrain(atoi(arg), 0, RGB_MODES - 1);    EEPROM.writeUChar(32, rgb_flag);    EEPROM.commit();  } else if (!strcmp(word, "BRI")) {    brightness = constrain(atoi(arg), 0, 255);    strip.setBrightness(brightness);    EEPROM.writeUChar(28, brightness);    EEPROM.commit();  } else if (!strcmp(word, "COL")) {    int r = 0, g = 0, b = 0;    sscanf(arg, "%d %d %d", &r, &g, &b);    solidR = constrain(r, 0, 255);    solidG = constrain(g, 0, 255);    solidB = constrain(b, 0, 255);    rgb_flag = 8;    saveTune();  } else if (!strcmp(word, "SET")) {    char key[16] = {0};    float v = 0;    if (sscanf(arg, "%15s %f", key, &v) == 2 && setTune(key, v)) reply = String("OK ") + key;    else reply = "ERR set";  } else if (!strcmp(word, "ANG")) {      // test: what the wheel sensor reads    char b[96];    snprintf(b, sizeof(b), "ANG %.1f deg  sensorVel %.2f  dir %d  motorOk %d", sensor.getAngle() * 57.2958,             sensor.getVelocity(), (int)motor.sensor_direction, motorOk);    reply = b;  } else if (!strcmp(word, "SPIN")) {     // test: turn the wheel without the sensor    float v = atof(arg);    if (v == 0) {      spinTest = false;      motor.voltage_limit = 12;      motor.controller = MotionControlType::torque;      if (!motorOk || mode == M_OFF) motor.disable();    } else {      stopShow();      spinTest = true;      spinVelocity = constrain(v, -30.0f, 30.0f);      motor.voltage_limit = 3;      motor.controller = MotionControlType::velocity_openloop;      motor.enable();    }  } else if (!strcmp(word, "INIT")) {     // run the motor self-check again    spinTest = false;    motor.voltage_limit = 12;    motor.controller = MotionControlType::torque;    startMotor(1);    reply = motorOk ? "INIT ok" : "INIT failed";  } else if (!strcmp(word, "WIFI")) {    if (atoi(arg)) wifiStart();    else wifiStop();  } else if (!legacyCommand(c)) {    reply = String("ERR ") + c;  }}void handleMessages() {  Msg m;  while (xQueueReceive(inbox, &m, 0) == pdTRUE) {    clientIp = m.ip;    clientPort = m.port;    clientSeen = millis();    char *save = nullptr;    for (char *c = strtok_r(m.text, ";\r\n", &save); c; c = strtok_r(nullptr, ";\r\n", &save)) {      String reply;      runCommand(c, reply);      if (reply.length()) udp.writeTo((const uint8_t *)reply.c_str(), reply.length(), IPAddress(clientIp), clientPort);    }  }}void readSerial() {  static char line[64];  static uint8_t len = 0;  while (Serial.available()) {    char ch = Serial.read();    if (ch == '\n' || ch == '\r' || ch == ';') {      line[len] = 0;      if (len) {        String reply;        runCommand(line, reply);        if (reply.length()) Serial.println(reply);      }      len = 0;    } else if (len < sizeof(line) - 1) {      line[len++] = ch;    }  }}String statusLine() {  char b[200];  snprintf(b, sizeof(b), "S bat=%.2f ang=%.1f vel=%.1f mode=%s lean=%.1f rgb=%d bri=%d show=%d low=%d up=%d ta=%.2f mot=%d",           bat_voltage, pendulum_angle, motor.shaft_velocity, modeName(mode), leanNow, rgb_flag, brightness,           showNumber, battery_low, uprightSince ? 1 : 0, target_angle, motorOk);  return String(b);}void sendStatus(uint32_t now) {  if (now - lastStatus < 100) return;  lastStatus = now;  if (wifi_on && clientIp && now - clientSeen < 5000) {    String s = statusLine();    udp.writeTo((const uint8_t *)s.c_str(), s.length(), IPAddress(clientIp), clientPort);  }  if (wifi_on && legacyStream) {  // the original PC tuning program's format    char b[200];    snprintf(b, sizeof(b), "v%.2f,vq%.2f,p%.2f,t%.2f,k%.2f,g%.2f,VT%.2f,", motor.shaft_velocity, motor.voltage.q,             pendulum_angle, target_angle, kalAngleZ, gyroZrate, bat_voltage);    udp.writeTo((const uint8_t *)b, strlen(b), IPAddress(192, 168, 4, 2), UDP_PORT);  }}/* ---- Wi-Fi hotspot ---- */void onPacket(AsyncUDPPacket packet) {  Msg m;  size_t n = packet.length();  if (n > sizeof(m.text) - 1) n = sizeof(m.text) - 1;  memcpy(m.text, packet.data(), n);  m.text[n] = 0;  m.ip = (uint32_t)packet.remoteIP();  m.port = packet.remotePort();  xQueueSend(inbox, &m, 0);}void wifiStart() {  if (wifi_on) return;  uint64_t mac = ESP.getEfuseMac();  snprintf(apName, sizeof(apName), "Nikola-Triangle-%02X%02X", (uint8_t)(mac >> 32), (uint8_t)(mac >> 40));  WiFi.mode(WIFI_AP);  WiFi.softAP(apName, AP_PASSWORD, 6, 0, 2);  WiFi.setTxPower(WIFI_POWER_8_5dBm);  // the phone is close by; less power, less noise  if (!udp.listen(UDP_PORT)) Serial.println("UDP listen failed");  udp.onPacket(onPacket);  wifi_on = true;  Serial.printf("WIFI_ON %s %s\r\n", apName, WiFi.softAPIP().toString().c_str());}void wifiStop() {  if (!wifi_on) return;  udp.close();  WiFi.softAPdisconnect(true);  WiFi.mode(WIFI_OFF);  wifi_on = false;  Serial.println("WIFI_OFF");}/* ---- touch pads ---- */void touchAttach(int id, uint8_t pin) {  int v = touchRead(pin);  if (v <= threshold_top) {    touch_count[id]++;  } else {    if (touch_count[id] >= single_count && touch_count[id] < long_count) touch_touched[id] = 1;    else if (touch_count[id] >= long_count) touch_touched[id] = 2;    else touch_touched[id] = 0;    touch_count[id] = 0;  }}void readTouch() {  touchAttach(0, T2);  // "favourite" pad  touchAttach(1, T3);  // "coin" pad  touchAttach(2, T4);  // "like" pad  for (int i = 0; i < 3; i++) {    if (touch_touched[i] == 1) tapEvent(i);    else if (touch_touched[i] == 2) longEvent(i);    touch_touched[i] = 0;  }}void tapEvent(int id) {  switch (id) {    case 0:  // dimmer      brightness = brightness <= 15 ? 0 : brightness - 15;      break;    case 1:  // brighter      brightness = brightness >= 240 ? 255 : brightness + 15;      break;    case 2:  // lights on/off      if (rgb_flag) rgb_flag = 0;      else rgb_flag = EEPROM.readUChar(32) ? EEPROM.readUChar(32) : 1;      return;  }  strip.setBrightness(brightness);  EEPROM.writeUChar(28, brightness);  EEPROM.commit();}void longEvent(int id) {  switch (id) {    case 0:  // previous effect      rgb_flag = (rgb_flag + RGB_TOUCH_MODES - 1) % RGB_TOUCH_MODES;      break;    case 1:  // next effect      rgb_flag = (rgb_flag + 1) % RGB_TOUCH_MODES;      break;    case 2:  // hotspot on/off (the motor keeps running, unlike the original)      if (wifi_on) wifiStop();      else wifiStart();      return;  }  strip.setBrightness(brightness);  EEPROM.writeUChar(32, rgb_flag);  EEPROM.commit();}/* ---- battery ---- */void voltage_detection() {  bat_voltage = return_voltage_value(BAT_VOLTAGE_SENSE_PIN);  if (bat_voltage < 3.0) return;  // no reading at all: the board has no battery sensor, so never cut out  if (bat_voltage < min_voltage && !battery_low) {    battery_low = 1;    Serial.printf("%.2fV battery_low!!\r\n", bat_voltage);    stopShow();    if (mode == M_BRAKE) endBrake();    mode = M_OFF;    motor.disable();    overlay = 11;  // red breathing = charge me  } else if (battery_low && bat_voltage >= min_voltage + 0.5) {    battery_low = 0;  // charged again; stays off until GO/RISE    Serial.printf("%.2fV battery ok\r\n", bat_voltage);    overlay = -1;    digitalWrite(ACTIVE_PIN, HIGH);  }}double return_voltage_value(int pin_no) {  double tmp = 0;  analogSetPinAttenuation(pin_no, ADC_6db);  for (int i = 0; i < 20; i++) {    double adc = analogReadMilliVolts(pin_no) / 1000.0;    tmp += adc * R1_VOLTAGE / R2_VOLTAGE + adc;  }  double v = tmp / 20;  if (v != 0) v += 0.001;  return v;}/* ---- saved settings ---- */void loadSettings() {  // The original's settings live at 0-32; keep using them so the toy's own  // balance angle carries over.  bool blank = false;  for (int k = 0; k <= 24; k += 4) {    if (isnan(EEPROM.readFloat(k))) blank = true;  }  if (blank) {    Serial.println("first write");    EEPROM.writeFloat(0, target_angle);    EEPROM.writeFloat(4, swing_up_voltage);    EEPROM.writeFloat(8, swing_up_angle);    EEPROM.writeFloat(12, v_p_1);    EEPROM.writeFloat(16, v_i_1);    EEPROM.writeFloat(20, v_p_2);    EEPROM.writeFloat(24, v_i_2);    EEPROM.writeUChar(28, brightness);    EEPROM.writeUChar(32, rgb_flag);    EEPROM.commit();  } else {    target_angle = EEPROM.readFloat(0);    swing_up_voltage = EEPROM.readFloat(4);    swing_up_angle = EEPROM.readFloat(8);    v_p_1 = EEPROM.readFloat(12);    v_i_1 = EEPROM.readFloat(16);    v_p_2 = EEPROM.readFloat(20);    v_i_2 = EEPROM.readFloat(24);    brightness = EEPROM.readUChar(28);    rgb_flag = EEPROM.readUChar(32);    if (rgb_flag >= RGB_MODES) rgb_flag = 1;  }  if (EEPROM.readUChar(TUNE_ADDR) == TUNE_MAGIC) {    EEPROM.get(TUNE_ADDR + 4, tune);    solidR = EEPROM.readUChar(TUNE_ADDR + 4 + sizeof(Tune));    solidG = EEPROM.readUChar(TUNE_ADDR + 5 + sizeof(Tune));    solidB = EEPROM.readUChar(TUNE_ADDR + 6 + sizeof(Tune));  }  Serial.printf("Balance angle %.2f, swing %.2fV/%.1f deg, brightness %d, effect %d\r\n", target_angle,                swing_up_voltage, swing_up_angle, brightness, rgb_flag);}void saveTune() {  EEPROM.writeUChar(TUNE_ADDR, TUNE_MAGIC);  EEPROM.put(TUNE_ADDR + 4, tune);  EEPROM.writeUChar(TUNE_ADDR + 4 + sizeof(Tune), solidR);  EEPROM.writeUChar(TUNE_ADDR + 5 + sizeof(Tune), solidG);  EEPROM.writeUChar(TUNE_ADDR + 6 + sizeof(Tune), solidB);  EEPROM.writeUChar(32, rgb_flag);  EEPROM.commit();}
lights.h · 197 lines Download

The 11 light effects for the 21 colour lights.

// The 21 colour lights around the triangle (WS2812 on GPIO16).// Effects 0-7 are the original firmware's (the touch pads cycle through those);// 8-10 are new and can be picked from the phone. 11-12 are only used by the shows.#include <Adafruit_NeoPixel.h>#define LED_PIN   16#define LED_COUNT 21Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);uint8_t brightness = 30;int rgb_flag = 1;               // the effect the user picked (0 = lights off)const int RGB_TOUCH_MODES = 8;  // effects the touch pads cycle through (same as the original)const int RGB_MODES = 11;       // effects the phone can pickuint8_t solidR = 0, solidG = 140, solidB = 255;int overlay = -1;               // effect forced by a show, -1 = noneunsigned long pixelPrevious = 0;int pixelInterval = 50;int pixelQueue = 0;int pixelCycle = 0;uint16_t pixelCurrent = 0;uint16_t pixelNumber = LED_COUNT;int lastEffect = -2;// 0-255 around the colour wheel: red -> green -> blue -> red.uint32_t Wheel(byte WheelPos) {  WheelPos = 255 - WheelPos;  if (WheelPos < 85) return strip.Color(255 - WheelPos * 3, 0, WheelPos * 3);  if (WheelPos < 170) {    WheelPos -= 85;    return strip.Color(0, WheelPos * 3, 255 - WheelPos * 3);  }  WheelPos -= 170;  return strip.Color(WheelPos * 3, 255 - WheelPos * 3, 0);}void colorWipe_delay(uint32_t c, uint8_t wait) {  for (uint16_t i = 0; i < strip.numPixels(); i++) {    strip.setPixelColor(i, c);    strip.show();    delay(wait);  }}// Two rainbow comets chasing round.void strip1() {  pixelInterval = 100;  strip.fill(0);  for (int i = 0; i < 5; i++) {    strip.setPixelColor((i + pixelNumber - pixelCycle) % pixelNumber, Wheel((i * 10 + pixelQueue) & 255));    strip.setPixelColor((i + pixelNumber - pixelCycle + 11) % pixelNumber, Wheel((i * 10 + pixelQueue + 128) & 255));  }  strip.show();  if (++pixelCycle >= pixelNumber) pixelCycle = 0;  if (++pixelQueue >= 256) pixelQueue = 0;}// Two white comets one way...void strip2() {  strip.fill(0);  for (int i = 0; i < 5; i++) {    int j = (4 - i) % 7 * (150 / 5) + 1;    strip.setPixelColor((i + pixelNumber - pixelCycle) % pixelNumber, strip.Color(j, j, j));    strip.setPixelColor((i + pixelNumber - pixelCycle + 11) % pixelNumber, strip.Color(j, j, j));  }  strip.show();  if (++pixelCycle >= pixelNumber) pixelCycle = 0;}// ...and the other way.void strip3() {  strip.fill(0);  for (int i = 0; i < 5; i++) {    int j = i % 7 * (150 / 5) + 1;    strip.setPixelColor((i + pixelCycle) % pixelNumber, strip.Color(j, j, j));    strip.setPixelColor((i + pixelCycle + 11) % pixelNumber, strip.Color(j, j, j));  }  strip.show();  if (++pixelCycle >= pixelNumber) pixelCycle = 0;}void rainbow1() {  pixelInterval = 30;  for (uint16_t i = 0; i < pixelNumber; i++) strip.setPixelColor(i, Wheel((i + pixelCycle) & 255));  strip.show();  if (++pixelCycle >= 256) pixelCycle = 0;}void rainbow2() {  pixelInterval = 30;  pixelCycle += 256;  strip.rainbow(pixelCycle);  strip.show();  if (pixelCycle >= 5 * 65536) pixelCycle = 0;}void pulse_rainbow1() {  pixelInterval = 30;  if (++pixelQueue >= brightness * 2) pixelQueue = 0;  if (pixelQueue < brightness) strip.setBrightness(pixelQueue);  else strip.setBrightness(brightness + brightness - pixelQueue);  pixelCycle += 256;  strip.rainbow(pixelCycle);  strip.show();  if (pixelCycle >= 5 * 65536) pixelCycle = 0;}void rgb_off() {  pixelInterval = 100;  strip.fill(0);  strip.show();}// 8: one colour, picked on the phone.void solid() {  pixelInterval = 100;  strip.fill(strip.Color(solidR, solidG, solidB));  strip.show();}// 9: red and blue halves swapping.void police() {  pixelInterval = 110;  pixelCycle++;  bool flip = (pixelCycle / 3) % 2;  bool on = pixelCycle % 3 != 2;  for (uint16_t i = 0; i < pixelNumber; i++) {    bool left = i < pixelNumber / 2;    uint32_t c = (left ^ flip) ? strip.Color(255, 0, 0) : strip.Color(0, 0, 255);    strip.setPixelColor(i, on ? c : 0);  }  strip.show();}// 10: random colour sparkles.void party() {  pixelInterval = 60;  for (uint16_t i = 0; i < pixelNumber; i++) {    strip.setPixelColor(i, random(3) ? Wheel(random(256)) : 0);  }  strip.show();}// 11 (shows only): slow red breathing.void redPulse() {  pixelInterval = 25;  pixelQueue = (pixelQueue + 4) % 512;  int v = pixelQueue < 256 ? pixelQueue : 511 - pixelQueue;  strip.fill(strip.Color(v, 0, 0));  strip.show();}// 12 (shows only): all lights flash white.void flashWhite() {  pixelInterval = 70;  pixelCycle++;  strip.fill(pixelCycle % 2 ? strip.Color(255, 255, 255) : 0);  strip.show();}// Called every loop; draws a frame when the current effect is due.void lightsUpdate(float wheelVelocity) {  unsigned long now = millis();  if (now - pixelPrevious < (unsigned long)pixelInterval) return;  pixelPrevious = now;  int e = overlay >= 0 ? overlay : rgb_flag;  if (e != lastEffect) {            // pulse_rainbow1 changes the brightness, so put it back    strip.setBrightness(brightness);    lastEffect = e;  }  switch (e) {    case 0: rgb_off(); break;    case 1:                           // comets that follow the wheel's spin      if (wheelVelocity > 0) {        pixelInterval = max(10, 150 - (int)wheelVelocity);        strip2();      } else {        pixelInterval = max(10, 150 + (int)wheelVelocity);        strip3();      }      break;    case 2: pixelInterval = 100; strip2(); break;    case 3: pixelInterval = 100; strip3(); break;    case 4: strip1(); break;    case 5: rainbow1(); break;    case 6: rainbow2(); break;    case 7: pulse_rainbow1(); break;    case 8: solid(); break;    case 9: police(); break;    case 10: party(); break;    case 11: redPulse(); break;    case 12: flashWhite(); break;    default: rgb_off(); break;  }}
i2c.ino · 63 lines Download

Talks to the tilt sensor.

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). Contact information ------------------- Kristian Lauszus, TKJ Electronics Web      :  http://www.tkjelectronics.com e-mail   :  kristianl@tkjelectronics.com */const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCBconst uint16_t I2C_TIMEOUT = 10; // Used to check for errors in I2C communicationuint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {  return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success}uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {  Wire.beginTransmission(IMUAddress);  Wire.write(registerAddress);  Wire.write(data, length);  uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success  if (rcode) {    Serial.print(F("i2cWrite failed: "));    Serial.println(rcode);  }  return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission}uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {  uint32_t timeOutTimer;  Wire.beginTransmission(IMUAddress);  Wire.write(registerAddress);  uint8_t rcode = Wire.endTransmission(false); // Don't release the bus  if (rcode) {    Serial.print(F("i2cRead failed: "));    Serial.println(rcode);    return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission  }  Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading  for (uint8_t i = 0; i < nbytes; i++) {    if (Wire.available())      data[i] = Wire.read();    else {      timeOutTimer = micros();      while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());      if (Wire.available())        data[i] = Wire.read();      else {        Serial.println(F("i2cRead timeout"));        return 5; // This error value is not already taken by endTransmission      }    }  }  return 0; // Success}
Kalman.h · 59 lines Download

Kalman filter by TKJ Electronics: smooths the tilt readings.

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). Contact information ------------------- Kristian Lauszus, TKJ Electronics Web      :  http://www.tkjelectronics.com e-mail   :  kristianl@tkjelectronics.com */#ifndef _Kalman_h_#define _Kalman_h_class Kalman {public:    Kalman();    // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds    float getAngle(float newAngle, float newRate, float dt);    void setAngle(float angle); // Used to set angle, this should be set as the starting angle    float getRate(); // Return the unbiased rate    /* These are used to tune the Kalman filter */    void setQangle(float Q_angle);    /**     * setQbias(float Q_bias)     * Default value (0.003f) is in Kalman.cpp.      * Raise this to follow input more closely,     * lower this to smooth result of kalman filter.     */    void setQbias(float Q_bias);    void setRmeasure(float R_measure);    float getQangle();    float getQbias();    float getRmeasure();private:    /* Kalman filter variables */    float Q_angle; // Process noise variance for the accelerometer    float Q_bias; // Process noise variance for the gyro bias    float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise    float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector    float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector    float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate    float P[2][2]; // Error covariance matrix - This is a 2x2 matrix};#endif
Kalman.cpp · 93 lines Download
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved. This software may be distributed and modified under the terms of the GNU General Public License version 2 (GPL2) as published by the Free Software Foundation and appearing in the file GPL2.TXT included in the packaging of this file. Please note that GPL2 Section 2[b] requires that all works based on this software must also be made publicly available under the terms of the GPL2 ("Copyleft"). Contact information ------------------- Kristian Lauszus, TKJ Electronics Web      :  http://www.tkjelectronics.com e-mail   :  kristianl@tkjelectronics.com */#include "Kalman.h"Kalman::Kalman() {    /* We will set the variables like so, these can also be tuned by the user */    Q_angle = 0.001f;    Q_bias = 0.003f;    R_measure = 0.03f;    angle = 0.0f; // Reset the angle    bias = 0.0f; // Reset bias    P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical    P[0][1] = 0.0f;    P[1][0] = 0.0f;    P[1][1] = 0.0f;};// The angle should be in degrees and the rate should be in degrees per second and the delta time in secondsfloat Kalman::getAngle(float newAngle, float newRate, float dt) {    // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145    // Modified by Kristian Lauszus    // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it    // Discrete Kalman filter time update equations - Time Update ("Predict")    // Update xhat - Project the state ahead    /* Step 1 */    rate = newRate - bias;    angle += dt * rate;    // Update estimation error covariance - Project the error covariance ahead    /* Step 2 */    P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);    P[0][1] -= dt * P[1][1];    P[1][0] -= dt * P[1][1];    P[1][1] += Q_bias * dt;    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")    // Calculate Kalman gain - Compute the Kalman gain    /* Step 4 */    float S = P[0][0] + R_measure; // Estimate error    /* Step 5 */    float K[2]; // Kalman gain - This is a 2x1 vector    K[0] = P[0][0] / S;    K[1] = P[1][0] / S;    // Calculate angle and bias - Update estimate with measurement zk (newAngle)    /* Step 3 */    float y = newAngle - angle; // Angle difference    /* Step 6 */    angle += K[0] * y;    bias += K[1] * y;    // Calculate estimation error covariance - Update the error covariance    /* Step 7 */    float P00_temp = P[0][0];    float P01_temp = P[0][1];    P[0][0] -= K[0] * P00_temp;    P[0][1] -= K[0] * P01_temp;    P[1][0] -= K[1] * P00_temp;    P[1][1] -= K[1] * P01_temp;    return angle;};void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting anglefloat Kalman::getRate() { return this->rate; }; // Return the unbiased rate/* These are used to tune the Kalman filter */void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };float Kalman::getQangle() { return this->Q_angle; };float Kalman::getQbias() { return this->Q_bias; };float Kalman::getRmeasure() { return this->R_measure; };

Downloads

APK Triangle Remote phone app (Android)TriangleRemote.apk · 45 KBJoins the triangle's own Wi-Fi by itself. TXT GPL-3.0 licenceLICENSE-GPL3.txt · 35 KBThe licence this code is shared under.

Related projects

Wi-Fi Motion Alarm

Wi-Fi Motion Alarm

ArduinoHard
Mini Bike GPS DynoWork in progress

Mini Bike GPS Dyno

Raspberry PiHard