kramann.info
© Guido Kramann

Login: Passwort:










2.9 Testsoftware

2.9 Testsoftware (EN google-translate)

2.9 Oprogramowanie testowe (PL google-translate)

Die Testsoftware erledigt zwei Aufgaben:

The test software does two things:

Oprogramowanie testowe wykonuje dwie rzeczy:

Wird der Einachser hingelegt, geht er in einen Standby-Modus. Wird er aufgerichtet, beginnt der Regelalgorithmus zu arbeiten:

If the Einachser lays down, he goes into a standby mode. When he is raised, the control algorithm starts to work:

Jeśli Einachser kładzie się, przechodzi w tryb gotowości. Gdy zostanie podniesiony, algorytm kontroli zaczyna działać:

Zyklisch werden Sensorsignale über die I2C-Schnittstelle vom Sensor MPU6050 geholt und daraus das Stellsignal für die Servos zum Verhindern eines Verkippens gebildet.

Sensor signals become cyclical via the I2C interface of the MPU6050 sensor and from this the control signal for the servos to prevent a Tilting made.

Sygnały czujnika stają się cykliczne poprzez interfejs I2C czujnika MPU6050 i od tego sygnał sterujący serwomechanizmów, aby zapobiec a Wykonano przechylanie.

Realisiert wurde hier ein PI-Regler, wobei als Besonderheit hier der P-Anteil quadriert wird, um dem Umstand Rechnung zu tragen, dass das Gefährt bei großen Abweichungen überproportional reagieren sollte, aber nahe an der Senkrechten sehr viel vorsichtiger.

Realized here was a PI controller, with a special feature here the P-share is squared to account for the circumstance that the vehicle should react disproportionately with large deviations, but very much more careful near the vertical.

Zrealizowany tutaj był kontroler PI, ze specjalną funkcją tutaj udział P jest wyrównany, aby uwzględnić tę okoliczność pojazd powinien reagować nieproporcjonalnie w przypadku dużych odchyleń, ale o wiele bardziej ostrożny w pobliżu pionu.

Achtung: Es werden Offsets von +/-20 auf das PWM-Signal gegeben, um eine Ungenauigkeit beim Fixieren der Servo-Mittelstellung auszugleichen. Diese Werte müßten von Serco zu Servo angepaßt werden.

Attention: Offsets of / -20 are given to the PWM signal to to compensate for an inaccuracy in fixing the servo center position. These values ​​would have to be adapted from Serco to Servo.

Uwaga: Przesunięcia / -20 są podawane do sygnału PWM do aby zrekompensować niedokładność w ustalaniu położenia środkowego serwomechanizmu. Wartości te musiałyby zostać dostosowane z Serco do Servo.

Es werden im aktuellen Programm lediglich die Beschleunigungsdaten als Rohdaten verwendet, aber nicht die Gyro-Daten.

In the current program, only the acceleration data is used as raw data, but not the gyro data.

W bieżącym programie tylko dane przyspieszenia są używane jako dane surowe, ale nie dane żyroskopowe.

einachser025.zip - Testsoftware
//Mittelwert omega=0.0348
//0.0485
//-0.0063
//LEDs:
//rot  A0+ A1-
//gelb A2+ A3-
//grün A4+ A5-

//A0 = D18
//A1 = D19
//A2 = D20
//A3 = D21
//A4 = D22
//A5 = D23

#include<math.h>
#include<Wire.h>

#define GYROSCALE 131.0
#define GYROOFFSET 2.71 //korrigierter Gyrooffset

//4.294.967.295  unsigned long
//    1.000.000  == 1s
//4.000.000.000  == 4000s
//1s == 1000ms == 1.000.000us


const int MPU=0x68;  // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

double ax,ay,az;
double arx,ary,arz,arx_alt;

double wx,wy,wz; //Winkelgeschwindigkeiten
double integ_arx=0.0;
double stellgroesse;
boolean SERIELL = false;
double m=0.0;

#include "Antrieb.h"

Antrieb antrieb;

void rot()
{
    digitalWrite(18,1);  
    digitalWrite(20,0);  
    digitalWrite(22,0);  
}

void gelb()
{
    digitalWrite(18,0);  
    digitalWrite(20,1);  
    digitalWrite(22,0);  
}

void gruen()
{
    digitalWrite(18,0);  
    digitalWrite(20,0);  
    digitalWrite(22,1);  
}

void aus()
{
    digitalWrite(18,0);  
    digitalWrite(20,0);  
    digitalWrite(22,0);  
}

void rotgruengelb()
{
    digitalWrite(18,1);  
    digitalWrite(20,1);  
    digitalWrite(22,1);  
}



void setup() 
{  
  pinMode(0,OUTPUT);  
  digitalWrite(0,0); //AD0 auf Masse
  
  pinMode(1,INPUT);  
  pinMode(4,INPUT);  //GND für MPU6050
  
  pinMode(5,INPUT);  
  pinMode(6,INPUT);  
  pinMode(7,INPUT);  
  pinMode(8,INPUT);  
  pinMode(9,INPUT);  
  pinMode(10,INPUT);  
  pinMode(11,INPUT);  
  pinMode(12,INPUT);  
  pinMode(13,INPUT);  
  // put your setup code here, to run once:
 Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(9600);

  pinMode(18,OUTPUT);  
  pinMode(19,OUTPUT);  
  pinMode(20,OUTPUT);  
  pinMode(21,OUTPUT);  
  pinMode(22,OUTPUT);  
  pinMode(23,OUTPUT);  

  digitalWrite(18,0);
  digitalWrite(19,0);
  digitalWrite(20,0);
  digitalWrite(21,0);
  digitalWrite(22,0);
  digitalWrite(23,0);
  
  antrieb.start();    

  arx_alt=0.0;  
}

void loop() 
{
  // put your main code here, to run repeatedly:
  Wire.beginTransmission(MPU);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,6,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)     
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  
  ax = (double)AcX;
  ay = (double)AcY;
  az = (double)AcZ;
  
  arx = ax / sqrt(ay*ay + az*az); 

  if(arx>arx_alt+0.03 || arx<arx_alt-0.03)
      arx = 0.82*arx_alt + 0.18*arx;


  stellgroesse = 18000.0*arx*arx;
  if(arx<0.0)
      stellgroesse = -stellgroesse;
  stellgroesse += integ_arx*10.0;

  arx_alt = arx;

  integ_arx+=arx;

  if(stellgroesse>480.0)
      stellgroesse=480.0;
  if(stellgroesse<-480.0)
      stellgroesse=-480.0;

  if(SERIELL)
  {
      Serial.println(" Berechnete Daten ohne atan: ");
      Serial.print(" arx = "); Serial.println(arx,DEC);
      Serial.print(" wz = "); Serial.println(wz,DEC);
      Serial.print(" stellgroesse = "); Serial.println(stellgroesse,DEC);
      delay(333);
  }

  if(arx<-0.7 || arx>0.7)
  {
      rot();
      antrieb.links(-20);
      antrieb.rechts(20);
      integ_arx=0.0;
  }
  else
  {
      gruen();
      antrieb.links((int)stellgroesse-20);
      antrieb.rechts((int)stellgroesse+20);      
  }
  
  //Auswertung:
  //es wird nur wz und arx benötigt
  //Drehung in Fahrtrichtung: wz<0 (Mathematisch positiver Sinn)
  //Verkippung in Fahrtrichtung: arx<0 (Mathematisch positiver Sinn)
  //OFFSET wz: ungefähr -2.6
}

Code 2.9-1: einachser025.ino - Hauptprogramm

#include "Arduino.h"

#define ANTRIEB_WMIN 1000
#define ANTRIEB_WMITTE 1500
#define ANTRIEB_WMAX 2000
#define ANTRIEB_SCHRITTE 1000

#define ANTRIEB_BEREICH 500

//Mode 8
//Phasen- und Frequenz-korrekt
//WGM1   3 2 1 0
//       1 0 0 0
//ICR1=..... TOP
//fpwm = fclk/(2*N*TOP)
//Vorteilung
//N=8
//80Hz = 16000000/(2*8*TOP)
//TOP = 16000000/(2*8*80Hz)=12500

//dt==1000ms*(1/80Hz)/12500 == 0,001ms (1 Schritt == 0,001ms)
//=>
//1ms == 1000 Schritte
//1,5ms == 1500 Schritte
//2ms == 2000 Schritte
class Antrieb
{
    public:
        Antrieb()
        {
         
        }  

        void start()
        {
            TCCR1A = (1<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0) | (0<<COM1C1) | (0<<COM1C0) | (0<<WGM11) | (0<<WGM10);
            TCCR1B = (0<<ICNC1) | (0<<ICES1) | (1<<WGM13) | (0<<WGM12) | (0<<CS12) | (1<<CS11) | (0<<CS10); //Vort. 256, s.S. 125
       
            ICR1=12500;
       
            DDRB |= (1<<PB5); //OCR1A
            DDRB |= (1<<PB6); //OCR1B
            OCR1A = ANTRIEB_WMITTE; //PWM-Breite auf Mitte setzen.  
            OCR1B = ANTRIEB_WMITTE; //PWM-Breite auf Mitte setzen.           
        }

        //x == -500...+500        
        void links(int x)
        {
             if(x<-ANTRIEB_BEREICH)
                 x=-ANTRIEB_BEREICH;
             if(x>ANTRIEB_BEREICH)
                 x=ANTRIEB_BEREICH;
             OCR1A = ANTRIEB_WMITTE+x;
        }
        
        void rechts(int x)
        {
             if(x<-ANTRIEB_BEREICH)
                 x=-ANTRIEB_BEREICH;
             if(x>ANTRIEB_BEREICH)
                 x=ANTRIEB_BEREICH;
             OCR1B = ANTRIEB_WMITTE-x;
        }
};

Code 2.9-2: Antrieb.h - Klasse zur Ansteuerung der Antriebsservos