Self Balancing Cube (Cubli) --> Program stopped at some point

Hello Guys,

I’ve got some struggles with my Arduino right now.
The thing is: I’m trying to make a similar Project as the Cubli ( link: The Cubli: a cube that can jump up, balance, and 'walk' - YouTube ). But at the stage of the prototype (1 Dimensional only) i cant do any further, because my program seems to stop or to stuck at a random stage.

So when i start it everything seems to work fine.
Then i get in case 1 and everything works just fine… accelerating the motor and then jumping in case 2. In case 2 it starts balancing quite good, but i cant adjust it better, because after a few secondes (always different time periods) my program gets stuck. At this point my button doesnt work any more and the motor wont stop acellerating/decelerating until i restart the Arduino.

Does anybody know why this happens all the time?

Yours Sincerely
Markus Kaser
Student of the technical College Wels for Mechatronics

Here’s my code:

#include <Wire.h>
#include <MPU6050_tockn.h>

int cycle = 0, vactual, az = 0;
const int motor = 9, button = 2, vneutral = 150, vstart = (vneutral + 8), vmin = 100, vmax = 200, inputmultiplicator = 10000;

unsigned long curr, prevaccelerate, prevsample;
const unsigned long accelerationtime = 2000;
unsigned long currmicros, prevmicros;

const float P = 0.2, I = 0, D = 0, abtastzeit = 10, winkelsoll = 0;

MPU6050 mpu6050(Wire);

float PIDregler(int angle, float soll, int Ta, unsigned long microsnow, unsigned long microsprevious, float Kp, float Ki, float Kd, int vlow, int vhigh, int vu) {
  float v, vout, deltav, error, erroralt, errorsum, ist = angle;
  
  if ((microsnow - microsprevious) >= Ta) {
    error = -(soll - ist) * vu;
    errorsum = errorsum + error;
    deltav = Kp * error + Ki * Ta * errorsum + Kd / Ta * (error - erroralt);
    erroralt = error;
    v = v + deltav;
    microsprevious = microsnow;
  
  vout = map(int(v), -32768, 32767, vmin, vmax);

  if (errorsum >= 32767 || v >= 999999 || v <= -999999) {
    errorsum = 0;
    v = 0;
  }

  if (vout <= vlow) {
    return vlow;
  }
  else if (vout >= vhigh) {
    return vhigh;
  }
  else {
    return vout;
  }
}
//Serial.println(omega);
}


void interrupt() {
  if (cycle == 0) {
    prevaccelerate = curr;
    cycle = 1;
  }
  else {
    cycle = 0;
  }
}

void setup() {
  pinMode(button, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(button), interrupt, FALLING);

  pinMode(motor, OUTPUT);

  Serial.begin(115200);
  Wire.begin();
  mpu6050.begin();
  mpu6050.setGyroOffsets(0.69, -2.43, -0.90);
}

void loop() {
  curr = millis();
  currmicros = millis();

  if((curr-prevsample)>=abtastzeit){
  mpu6050.update();
  az = mpu6050.getAngleZ();
  prevsample=curr;
  }

  switch (cycle) {
    case 0:
      vactual = vneutral;
      prevaccelerate = curr;
      break;

    case 1:
      vactual = vstart;
      if ((curr - prevaccelerate) >= accelerationtime) {
        prevmicros = currmicros;
        cycle = 2;
      }
      break;

    case 2:
      vactual = PIDregler(az, winkelsoll, abtastzeit, currmicros, &prevmicros, P, I, D, vmin, vmax, inputmultiplicator);
      break;
  }

  analogWrite(motor, vactual);

  Serial.println(az);
}

And a picture of my circuit is attached too.