self_balancierer

**piggituX** · 19.12.2017, 20:23

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

**Rabenauge** · 20.12.2017, 12:30

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).

**inka** · 19.01.2018, 15:46

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:

Klicke auf die Grafik für eine größere Ansicht

Name: IMG_20180119_2617.jpg
Hits: 8
Größe: 37,9 KB
ID: 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...

**Rabenauge** · 19.01.2018, 23:07

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.

Thema: self_balancierer

Themen-Optionen

Thema durchsuchen

Anzeige

Hybrid-Darstellung

Lesezeichen

Lesezeichen

Berechtigungen