roboforum.ru

void loop() {
  /* Update all the values */
  uint8_t* data = i2cRead(0x3B,14);  
  accX = ((data[0] << 8) | data[1]);
  accY = ((data[2] << 8) | data[3]);
  accZ = ((data[4] << 8) | data[5]);  
  gyroY = ((data[10] << 8) | data[11]);
  
  /* Calculate the angls based on the different sensors and algorithm */
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  
  double gyroYrate = -((double)gyroY/131.0);
  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);

  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
  
  in = kalAngleY;
  pid.Compute();
  if (out < 0) {
    pwm = -1 * out;
    analogWrite(11, 0);
    analogWrite(10, pwm);
  } else {
    pwm = out;
    analogWrite(10, 0);
    analogWrite(11, pwm);
  }
  
  delay(1); // The accelerometer's maximum samples rate is 1kHz
}

  if (Serial.available()) {
    int inByte = Serial.read();
    if (inByte=='1') {
      delta=10; // Приращение
      iter=50;  // Итерации
    }
    if (inByte=='0') {
      delta=-10;
      iter=50;
    }
  }
  if (iter>0) {
    iter--;
    out=out+delta;
  }