self_balancierer

hallo allerseits,

ich habe wieder mal etwas herumgebastelt, das hier ist das ergebnis:

Anhang 33129 Anhang 33130 Anhang 33131

das verhalten unter dem weiter unten aufgelisteten code kann man in diesem video sehen. Der robby wird oben festgehalten, man kann aber sehen, dass die regelung versucht dem "fallen" entgegen zu wirken...

das thema kompass (hier MPU6050) ist neu für mich, der code ist auch garnicht von mir, ich habe ihn nur im rahmen meiner möglichkeiten angepasst.

fragen/bemerkungen:

- die anpassung an die adafruit motorlib (was die initialisierung und ansteuerung betrifft) war kein problem

- das mit dem setMotors und motorPower (ab zeile 80 und 130) habe ich nur soweit angepasst, wie es mir für die adafruit lib notwendig schien

- die US entfernungs-messung ist für spätere verwendung auskommentiert

- die ISR routine haben ich so gelassen wie sie war. in dieser zeile:

accAngle = atan2(accX, accY) * RAD_TO_DEG;

wird z.b. ein winkel "in der fläche" (x und y bilden bei mir die waagerechte fläche) berechnet, warum reicht nicht die senkrechte richtung zu der drehachse der räder?

- die PID regelung wollte ich mit meinen bescheidenen kenntnissen der steuerungstechnik lieber nich anfassen...


und dann kommen schon die altbekannten funktionen der adafruit motoren lib...

was das verhalten beim balancieren betrifft, habe ich versucht es mit änderungen der motorengeschwindigkeit (motor_A->setSpeed(200)) und der dauer des motor-einsatzes (dauer_fahrt = 50) zu regeln, wahrscheinlich an der PID regelung völlig vorbei...

wo muss ich anfangen mit der anpassung? Was ist sinnvoll? Könnte sich jemand die zeit nehmen und versuchen mit mir step by step die zusammenhänge durchzugehen?

Code:

---

// http://www.instructables.com/id/Arduino-Self-Balancing-Robot-1/


// stammt vom "midhun_s"


// http://www.instructables.com/member/midhun_s/
#include "Arduino.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#include <NewPing.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"




Adafruit_MotorShield AFMS = Adafruit_MotorShield();


Adafruit_DCMotor *motor_A = AFMS.getMotor(3);
Adafruit_DCMotor *motor_B = AFMS.getMotor(4);


#define TRIGGER_PIN 6 //9
#define ECHO_PIN 7 //8
#define MAX_DISTANCE 175


#define Kp  40
#define Kd  0.05
#define Ki  40
#define sampleTime  0.005
#define targetAngle -2.5


MPU6050 mpu;
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);


int16_t accX, accY, accZ, gyroX, gyroY, gyroZ;
volatile int motorPower, gyroRate;
volatile float accAngle, gyroAngle, currentAngle, prevAngle = 0, error, prevError = 0, errorSum = 0;
volatile byte count = 0;
int distanceCm;
unsigned long dauer_fahrt, currentMillis, previousMillis;


void setup()
{
  // set the motor control and PWM pins to output mode
  AFMS.begin();


  // set the status LED to output mode
  pinMode(13, OUTPUT);


  // initialize the MPU6050 and set offset values
  mpu.initialize();


  mpu.setXAccelOffset(-1151);
  mpu.setYAccelOffset(-823);
  mpu.setZAccelOffset(888);


  mpu.setXGyroOffset(118);
  mpu.setYGyroOffset(28);
  mpu.setZGyroOffset(183);
  // initialize serial.print
  Serial.begin(115200);
  Serial1.begin(115200);


  // initialize PID sampling loop
  init_PID();
}


void loop()
{
  // read acceleration and gyroscope values
  accX = mpu.getAccelerationX();
  accY = mpu.getAccelerationY();
  accZ = mpu.getAccelerationZ();


  gyroX = mpu.getRotationX();
  gyroY = mpu.getRotationY();
  gyroZ = mpu.getRotationZ();


  accAngle = atan2(accX, accY) * RAD_TO_DEG; //accY, accZ


  // set motor power after constraining it
  motorPower = constrain(motorPower, -255, 255);
  setMotors(motorPower, motorPower);
/*
  // measure distance every 100 milliseconds
  if ((count % 20) == 0)
  {
    distanceCm = sonar.ping_cm();
  }
  if ((distanceCm < 20) && (distanceCm != 0))
  {
    setMotors(-motorPower, motorPower);
  }
*/
}




// The ISR will be called every 5 milliseconds
ISR(TIMER1_COMPA_vect)
{
  // calculate the angle of inclination
  accAngle = atan2(accX, accY) * RAD_TO_DEG; //accY, accZ
  gyroRate = map(gyroX, -32768, 32767, -250, 250);
  gyroAngle = (float)gyroRate * sampleTime;
  currentAngle = 0.9934 * (prevAngle + gyroAngle) + 0.0066 * (accAngle);


  error = currentAngle - targetAngle;
  errorSum = errorSum + error;
  errorSum = constrain(errorSum, -300, 300);
  //calculate output from P, I and D values
  motorPower = Kp * (error) + Ki * (errorSum) * sampleTime - Kd * (currentAngle - prevAngle) / sampleTime;
  prevAngle = currentAngle;
  // toggle the led on pin13 every second
  count++;
  if (count == 100)
  {
    count = 0;
    digitalWrite(13, !digitalRead(13));
/*
    Serial.print(gyroX);
    Serial.print("  ");
    Serial.println(accAngle);


    Serial1.print(gyroX);
    Serial1.print("  ");
    Serial1.println(accAngle);
*/
  }
}


void setMotors(int leftMotorSpeed, int rightMotorSpeed)
{
  if (leftMotorSpeed >= 0)
  {
    //rueckwaerts();
    vorwaerts();
  }
  else
  {
    //vorwaerts();
    rueckwaerts();
  }
  if (rightMotorSpeed >= 0)
  {
    //vorwaerts();
    rueckwaerts();
  }
  else
  {
    //rueckwaerts();
    vorwaerts();
  }
}


void init_PID()
{
  // initialize Timer1
  cli();          // disable global interrupts
  TCCR1A = 0;    // set entire TCCR1A register to 0
  TCCR1B = 0;    // same for TCCR1B
  // set compare match register to set sample time 5ms
  OCR1A = 9999;
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS11 bit for prescaling by 8
  TCCR1B |= (1 << CS11);
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);
  sei();          // enable global interrupts
}


void vorwaerts (void)
{
  currentMillis = millis();


  if (currentMillis - previousMillis > dauer_fahrt)
  {
  motor_A->setSpeed(200);
  motor_B->setSpeed(200);


  Serial.println("vorwaerts");
  Serial1.println("vorwaerts");




  motor_A->run(FORWARD);
  motor_B->run(FORWARD);
  dauer_fahrt = 50;
  }
  //delay(1000);
}


void rueckwaerts (void)
{
    currentMillis = millis();


    if (currentMillis - previousMillis > dauer_fahrt)
  {
  motor_A->setSpeed(200);
  motor_B->setSpeed(200);


  Serial.println("rueckwaerts");
  Serial1.println("rueckwaerts");


  motor_A->run(BACKWARD);
  motor_B->run(BACKWARD);


  dauer_fahrt = 50;
  }
  //delay(1000);
}


void motoren_stop(void)
{


  motor_A->setSpeed(0);
  motor_B->setSpeed(0);


  Serial.println("motoren stop");
  Serial1.println("motoren stop");


  motor_A->run(RELEASE);
  motor_B->run(RELEASE);


  //delay(1000);


}

---

Hi,

ich arbeite auch gerade nebenbei an einem Self Balanced mit ähnlichem Aufbau.... da ich leider nicht aus der WissenschaftsEcke komme, hänge ich gerade am reinen Verständnis der Balance. Mir bereitet gerade der Kalman Filter oder eben das Zusammenspeil des IMU mit den Motoren Kopfzerbrechen.

Ich lese gerade intensiv im Internet auf diversen Seiten um es zu verstehen, Am Besten finde ich folgende Seite.

https://roboticdreams.wordpress.com/...-robot-part-1/

Mein Aufbau...

Arduino UNO
Adafruit MotorLib V2.3
Adafruit 10DOF
NEMA 17 Stepper

Spiel mit den Werten Kp,Ki und Kd.
Nicht so an der Motorsteuerung herum pfuschen!

Ich hab das damals so gemacht,dass ich mir drei Potis vorübergehend eingebaut hab, je einen für die drei Parameter.
Kann man einfach mal an analoge Eingänge stecken...
So kann man zur Laufzeit herumprobieren. Die jeweils eingestellten Werte hab ich dann auf Display ausgegeben, um auch sehen zu können, in welchen Bereichen ich mich da bewege.

Stell zuerst Kp mal so ein, dass es schon "beinahe hinhaut"- komplett klappen wird das mit Kp alleine nicht!
Ki und Kd auf 0.
Wenn du ein leichtes Übersteuern der Regelung bemerkst, lass Kp so und mach mal vorsichtig mit Ki weiter...mit beiden zusammen sollte das schon "fast funktionieren".
Kd ist dann gewissermassen Feintuning.
Da die drei Werte sich gegenseitig beeinflussen, ist da einiges an Spielerei nötig, darum die Potis- sonst dauert das Wochen (wenn man keine Ahnung hat, in welchen Bereichen die drei sich überhaupt bewegen sollten).

ich habe jetzt erstmal ein code ohne PID ausprobiert:

Code:

---

#include <Arduino.h>
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#include <NewPing.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"


Adafruit_MotorShield AFMS = Adafruit_MotorShield();


Adafruit_DCMotor *motor_A = AFMS.getMotor(3);
Adafruit_DCMotor *motor_B = AFMS.getMotor(4);


#define TRIGGER_PIN 6 //9
#define ECHO_PIN 7 //8
#define MAX_DISTANCE 175


NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);


long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;


long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;


void setup()
{
  // initialize serial.print
  Serial.begin(115200);
  Serial1.begin(115200);
  Serial.println("AFMS test_1 ohne PID");
  Serial1.println("AFMS test_1 ohne PID");


  // initialize AFMS
  AFMS.begin();
  Wire.begin();
  TWBR = 12; // set 400kHz mode @ 16MHz CPU
 
  setupMPU();
}




void loop()
{
  static long Q_Timer = millis();
  if ((long)( millis() - Q_Timer ) >= 20)
  {
    Q_Timer = millis();
    recordAccelRegisters();
    recordGyroRegisters();
    printData();
    balance();
  }
}


void balance() //120
{
  if (gForceX >= 0.3)
  {


    static long M_Timer = millis();
    if ((long)( millis() - M_Timer ) >= 10)
    {
      M_Timer = millis();


      motor_A->setSpeed(120);
      motor_B->setSpeed(120);
      /*
            motor_A->run(RELEASE);
            motor_B->run(RELEASE);
      */
      delay(10);


      motor_A->run(BACKWARD);
      motor_B->run(BACKWARD);
    }
  }
  if (gForceX <= 0.3)
  {
    static long M_Timer = millis();
    if ((long)( millis() - M_Timer ) >= 10)
    {
      M_Timer = millis();


      motor_A->setSpeed(120);
      motor_B->setSpeed(120);
      /*
            motor_A->run(RELEASE);
            motor_B->run(RELEASE);
      */
      delay(10);


      motor_A->run(FORWARD);
      motor_B->run(FORWARD);
    }
  }
}






void setupMPU()
{
  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001
  //for AC0 low/high datasheet sec. 9.2)
  Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
  Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
  Wire.write(0b00000000); //Setting the accel to +/- 2g
  Wire.endTransmission();
}


void recordAccelRegisters()
{
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000, 6); //Request Accel Registers (3B - 40)
  while (Wire.available() < 6);
  accelX = Wire.read() << 8 | Wire.read(); //Store first two bytes into accelX
  accelY = Wire.read() << 8 | Wire.read(); //Store middle two bytes into accelY
  accelZ = Wire.read() << 8 | Wire.read(); //Store last two bytes into accelZ
  processAccelData();
}


void processAccelData()
{
  gForceX = accelX / 16384.0;
  gForceY = accelY / 16384.0;
  gForceZ = accelZ / 16384.0;
}


void recordGyroRegisters()
{
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x43); //Starting register for Gyro Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000, 6); //Request Gyro Registers (43 - 48)
  while (Wire.available() < 6);
  gyroX = Wire.read() << 8 | Wire.read(); //Store first two bytes into accelX
  gyroY = Wire.read() << 8 | Wire.read(); //Store middle two bytes into accelY
  gyroZ = Wire.read() << 8 | Wire.read(); //Store last two bytes into accelZ
  processGyroData();
}


void processGyroData()
{
  rotX = gyroX / 131.0;
  rotY = gyroY / 131.0;
  rotZ = gyroZ / 131.0;
}


void printData()
{


  Serial.print(" accelX=  ");
  Serial.println(gForceX);


  Serial1.print(" accelX=  ");
  Serial1.println(gForceX);


}

---

ohne mir zu arg den kopf über die einzelnen blöcke zerbrochen zu haben. Habe einfach angenommen, dass diese funktionieren, habe es an meine hardware angepasst und habe versucht die zusammenhänge ein bischen zu begreifen.

einen adapter mit drei potis habe ich auch schon angefertig:
Anhang 33222


Aber erstmal habe ich versucht die arduino PID-V1 library mit vorgegebenen Kp, Kd und Ki einzubauen:

Code:

---

#include <Arduino.h>
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
#include <NewPing.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_MS_PWMServoDriver.h"
#include <PID_v1.h>






Adafruit_MotorShield AFMS = Adafruit_MotorShield();


Adafruit_DCMotor *motor_A = AFMS.getMotor(3);
Adafruit_DCMotor *motor_B = AFMS.getMotor(4);


#define TRIGGER_PIN 6 //9
#define ECHO_PIN 7 //8
#define MAX_DISTANCE 175


NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);


/************************/
double Setpoint, Input, Output, motorPower, motorPowerSum;






PID myPID(&Input, &Output, &Setpoint, 40, 5, 1, REVERSE);




/***********************/


long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ;


long gyroX, gyroY, gyroZ;
float rotX, rotY, rotZ;




void setup()
{
  // initialize serial.print
  Serial.begin(115200);
  Serial1.begin(115200);
  Serial.println("AFMS test_1 mit PID library");
  Serial1.println("AFMS test_1 mit PID library");


  // initialize AFMS
  AFMS.begin();
  Wire.begin();
  TWBR = 12; // set 400kHz mode @ 16MHz CPU
  /*******************/
  // initialize PID sampling loop
  Setpoint = 0.3;
  //myPID.SetSampleTime(100);
  myPID.SetOutputLimits(-255, 255);
  myPID.SetMode(AUTOMATIC);
  /*******************/
  setupMPU();
}




void loop()
{
  static long Q_Timer = millis();
  if ((long)( millis() - Q_Timer ) >= 20)
  {
    Q_Timer = millis();
    recordAccelRegisters();
    recordGyroRegisters();


    Input = gForceX;
    myPID.Compute();
   
    analogWrite(3, Output);
   
    if (Output <=0)
    {
      motorPower = - Output;
    }
    else
    {
      motorPower = Output;
    }
   
    printData();
    balance();
  }
}


void balance() //motorPower
{
  if (gForceX >= 0.3)
  {


    static long M_Timer = millis();
    if ((long)( millis() - M_Timer ) >= 10)
    {
      M_Timer = millis();


      motor_A->setSpeed(motorPower);
      motor_B->setSpeed(motorPower);
      /*
            motor_A->run(RELEASE);
            motor_B->run(RELEASE);
      */
      delay(10);


      motor_A->run(BACKWARD);
      motor_B->run(BACKWARD);
    }
  }
  if (gForceX <= 0.3)
  {
    static long M_Timer = millis();
    if ((long)( millis() - M_Timer ) >= 10)
    {
      M_Timer = millis();


      motor_A->setSpeed(motorPower);
      motor_B->setSpeed(motorPower);
      /*
            motor_A->run(RELEASE);
            motor_B->run(RELEASE);
      */
      delay(10);


      motor_A->run(FORWARD);
      motor_B->run(FORWARD);
    }
  }
}






void setupMPU()
{
  Wire.beginTransmission(0b1101000); //This is the I2C address of the MPU (b1101000/b1101001
  //for AC0 low/high datasheet sec. 9.2)
  Wire.write(0x6B); //Accessing the register 6B - Power Management (Sec. 4.28)
  Wire.write(0b00000000); //Setting SLEEP register to 0. (Required; see Note on p. 9)
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1B); //Accessing the register 1B - Gyroscope Configuration (Sec. 4.4)
  Wire.write(0x00000000); //Setting the gyro to full scale +/- 250deg./s
  Wire.endTransmission();
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x1C); //Accessing the register 1C - Acccelerometer Configuration (Sec. 4.5)
  Wire.write(0b00000000); //Setting the accel to +/- 2g
  Wire.endTransmission();
}


void recordAccelRegisters()
{
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x3B); //Starting register for Accel Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000, 6); //Request Accel Registers (3B - 40)
  while (Wire.available() < 6);
  accelX = Wire.read() << 8 | Wire.read(); //Store first two bytes into accelX
  accelY = Wire.read() << 8 | Wire.read(); //Store middle two bytes into accelY
  accelZ = Wire.read() << 8 | Wire.read(); //Store last two bytes into accelZ
  processAccelData();
}


void processAccelData()
{
  gForceX = accelX / 16384.0;
  gForceY = accelY / 16384.0;
  gForceZ = accelZ / 16384.0;
}


void recordGyroRegisters()
{
  Wire.beginTransmission(0b1101000); //I2C address of the MPU
  Wire.write(0x43); //Starting register for Gyro Readings
  Wire.endTransmission();
  Wire.requestFrom(0b1101000, 6); //Request Gyro Registers (43 - 48)
  while (Wire.available() < 6);
  gyroX = Wire.read() << 8 | Wire.read(); //Store first two bytes into accelX
  gyroY = Wire.read() << 8 | Wire.read(); //Store middle two bytes into accelY
  gyroZ = Wire.read() << 8 | Wire.read(); //Store last two bytes into accelZ
  processGyroData();
}


void processGyroData()
{
  rotX = gyroX / 131.0;
  rotY = gyroY / 131.0;
  rotZ = gyroZ / 131.0;
}


void printData()
{


  Serial.print(" accelX=  ");
  Serial.print(gForceX);
  Serial.print(" Input=  ");
  Serial.print(Input);
  Serial.print(" Output=  ");
  Serial.print(Output);
  Serial.print(" motorPower=  ");
  Serial.println(motorPower);
  Serial.print(" motorPowerSum=  ");
  Serial.println(motorPowerSum);




  Serial1.print(" accelX=  ");
  Serial1.print(gForceX);
  Serial1.print(" Input=  ");
  Serial1.print(Input);
  Serial1.print(" Output=  ");
  Serial1.print(Output);
  Serial1.print(" motorPower=  ");
  Serial1.print(motorPower);
  Serial1.print(" motorPowerSum=  ");
  Serial1.println(motorPowerSum);


}

---

das hat auch halbwegs geklappt. Der "zweirädler" bzw. der code reagiert so, wie man es erwartet, will heissen, beim kippen um die achse der räder wird gegengesteuert. Während die ausgabe von "accelX" annähernd konstannt bleibt, erhöht sich der wert von "motorPower" kontinuierlich bis 255. Allerdings quälend langsam. Kippe ich den roboter in entgegengesetzte richtung, wird der wert von "motorPower" erstmal genauso langsam wieder runtergezählt, bevor er anfängt genauso langsam wieder hochzuzählen. Die änderung des kippwinkels von "+" in "-" ist in der tabelle ca. bei output=70 zu sehen.

(Die tabelle mit den ausgegebenen werten war zu lang, deshalb als video hier zu sehen...)

Code:

---

accelX=  -0.45 Input=  -0.45 Output=  -36.20 motorPower=  36.20 motorPowerSum=  0.00
 accelX=  -0.40 Input=  -0.40 Output=  -38.47 motorPower=  38.47 motorPowerSum=  0.00
 accelX=  -0.48 Input=  -0.48 Output=  -38.47 motorPower=  38.47 motorPowerSum=  0.00
 accelX=  -0.50 Input=  -0.50 Output=  -38.47 motorPower=  38.47 motorPowerSum=  0.00
 accelX=  -0.42 Input=  -0.42 Output=  -38.47 motorPower=  38.47 motorPowerSum=  0.00
 accelX=  -0.41 Input=  -0.41 Output=  -38.47 motorPower=  38.47 motorPowerSum=  0.00
 accelX=  -0.47 Input=  -0.47 Output=  -42.38 motorPower=  42.38 motorPowerSum=  0.00
 accelX=  -0.44 Input=  -0.44 Output=  -42.38 motorPower=  42.38 motorPowerSum=  0.00
 accelX=  -0.49 Input=  -0.49 Output=  -42.38 motorPower=  42.38 motorPowerSum=  0.00
 accelX=  -0.50 Input=  -0.50 Output=  -42.38 motorPower=  42.38 motorPowerSum=  0.00
 accelX=  -0.43 Input=  -0.43 Output=  -42.38 motorPower=  42.38 motorPowerSum=  0.00
 accelX=  -0.49 Input=  -0.49 Output=  -42.93 motorPower=  42.93 motorPowerSum=  0.00
 accelX=  -0.39 Input=  -0.39 Output=  -42.93 motorPower=  42.93 motorPowerSum=  0.00
 accelX=  -0.43 Input=  -0.43 Output=  -42.93 motorPower=  42.93 motorPowerSum=  0.00
 accelX=  -0.44 Input=  -0.44 Output=  -42.93 motorPower=  42.93 motorPowerSum=  0.00
 accelX=  -0.45 Input=  -0.45 Output=  -42.93 motorPower=  42.93 motorPowerSum=  0.00
 accelX=  -0.50 Input=  -0.50 Output=  -43.62 motorPower=  43.62 motorPowerSum=  0.00

---

Meine frage:
wie erreiche ich, dass die umschaltung beim kippwinkel schneller erfolgt? Das muss ja irgendwo bei der umsetzung des "output" wertes zu "motorPower" passieren, nur ich habe keinen plan wie. Ich habe in diesem bereich:

Code:

---

void balance() //motorPower
{
  if (gForceX >= 0.3)
  {


    static long M_Timer = millis();
    if ((long)( millis() - M_Timer ) >= 10)
    {
      M_Timer = millis();


      motor_A->setSpeed(motorPower);
      motor_B->setSpeed(motorPower);
      /*
            motor_A->run(RELEASE);
            motor_B->run(RELEASE);
      */
      delay(10);


      motor_A->run(BACKWARD);
      motor_B->run(BACKWARD);
    }
  }
  if (gForceX <= 0.3)
  {
    static long M_Timer = millis();
    if ((long)( millis() - M_Timer ) >= 10)
    {
      M_Timer = millis();


      motor_A->setSpeed(motorPower);
      motor_B->setSpeed(motorPower);
      /*
            motor_A->run(RELEASE);
            motor_B->run(RELEASE);
      */
      delay(10);


      motor_A->run(FORWARD);
      motor_B->run(FORWARD);
    }
  }
}

---

EDIT: und hier:

Code:

---

void loop()
{
  static long Q_Timer = millis();
  if ((long)( millis() - Q_Timer ) >= 20)
  {
    Q_Timer = millis();
    recordAccelRegisters();
    recordGyroRegisters();


    Input = gForceX;
    myPID.Compute();
   
    analogWrite(3, Output);
   
    if (Output <=0)
    {
      motorPower = - Output;
    }
    else
    {
      motorPower = Output;
    }
   
    printData();
    balance();
  }
}

---

schon alle möglichen varianten ausprobiert, keine wars...

Ich hab leider mein altes Programm nicht mehr, finde aber beim überfliegen deines jetzt nirgends Werte für Kp, Ki und Kd?

Das, was du beschreibst, deutet drauf hin, dass die Regelung viel zu träge reagiert.
Da müsste -erstmal- die Erhöhung von Kp Abhilfe schaffen.

Was aber auch wichtig ist: Tempo. Daher ist es keine so gute Idee, die serielle Konsole zu benutzen- die bremst gewaltig!
Das Ganze muss möglichst schnell laufen, je früher die Regelung auf Abweichungen reagiert, umso besser.

die PID-werte sind hier:

PID myPID(&Input, &Output, &Setpoint, 40, 5, 1, REVERSE);

mit dem reverse bin ich nicht sicher, vielleicht doch direct? Jetzt baue ich mal den potiadapter ein...