self_balancierer

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

Thema: self_balancierer

Themen-Optionen

Thema durchsuchen

Anzeige

Baum-Darstellung

Lesezeichen

Lesezeichen

Berechtigungen