Projekt 57: Arduino Segway

Autoren: Philipp Tewes, Janis Ostermann
Betreuer: Prof. Dr. Mirek Göbel

Arduino-Segway

Aufgabenstellung

• Bauen eines Segways auf Basis eines Arduinos
• Komplett eigene Konstruktion

Das Projekt wurde von Philipp Tewes und Janis Ostermann im Rahmen des Elektrotechnik Fachpraktikums im WS 2015/16 bearbeitet. Zusätzlich zu diesem Artikel befinden sich weitere Dateien, wie zum Beispiel die Segway-Bauanleitung und vollständiger Programmcode, im SVN.

Ziele

Text...

Bau

Text...

Sensor mit abgelöteten Widerständen und Transistoren

Sensor mit passender LEGO-Buchse

Teile Liste

Text...

Etagen

Text...

Etage 1

Text...

Etage 2

Text...

Etage 3

Text...

Steuerbox

Text...

Montage

Text...

Verkabelung

Text...

Reifen

Text...

Software des Arduino UNO

Programmcode

#include "Wire.h"
#include "SPI.h"  
#include "nRF24L01.h"
#include "I2Cdev.h"
#include "MPU6050.h"

// Variablen für das Accelometer und den Gyro
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

// Definitionen von Schlüsselwörtern
#define Gry_offset 300                   // Offset des Gyro
#define Gyro_Verstaerkung 0.00763358     // Verstärkung des Gyro Analog Signal
#define Winkel_offset -22                // Offset des Aktuellen Winkels 
#define Motor_offset 0                   // Motor Offset
#define pi 3.14159                       // Definition von der zahl pi

// Variablen für den PID Regler
float kp, ki, kd; 
float r_angle, f_angle, omega;

// Variablen für die Zeitberechnung
unsigned long preTime = 0;
float SampleTime = 0.08;
unsigned long lastTime;
float Output;
float errSum, dErr, error, lastErr;
int timeChange; 

// Definition der Ports mit Variable für Pinmode
//Reifen rechts
int TN1=8; //TN3=5;
int TN2=9; //TN4=6;
int ENA=5; //ENB=10;

//Reifen links
int TN3=11; //TN1=3;
int TN4=10; //TN2=4;
int ENB=6; //ENA=9;


int sensorPin0 = A0;    // Input Pin für das Potentiometer 0
int sensorPin1 = A1;    // Input Pin für das Potentiometer 1
int sensorPin2 = A2;    // Input Pin für das Potentiometer 2

int sensorValue1 = 0;   // Variable um den Wert des Potentiometer 0 zu speichern
int sensorValue0 = 0;   // Variable um den Wert des Potentiometer 1 zu speichern
int sensorValue2 = 0;   // Variable um den Wert des Potentiometer 2 zu speichern



void setup() {
  Wire.begin();             // Start serieller Kommunikation
  accelgyro.initialize();   // Initalisieren des Gyro/Accelometer
  pinMode(TN1,OUTPUT);      // Ausgänge zuweisen
  pinMode(TN2,OUTPUT);
  pinMode(TN3,OUTPUT);
  pinMode(TN4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);

  Serial.begin(115200);     // Serielle Bandrate 
}



void loop() 
{
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);  // Aktuellen wert des Gyro/Accelometer laden
  r_angle = (atan2(ay, az) * 180 / pi + Winkel_offset); // Aktueller Winkel (bei 0 Grad steht das Segway gerade)
  omega =  Gyro_Verstaerkung * (gx +  Gry_offset);  Serial.print("  omega="); Serial.println(omega);
  if (abs(r_angle)<45){ // Winkel kleiner als 45 Grad
    myPID(); // Berechnung der aktuelen PID Werte
    PWMControl(); // Ansteuern der Motoren mit aktuellen Werten
  }
  else{	// Wenn der winkel größer als 45 Grad wird soll angenommen werden, dass das Segway umgefallen ist und keine Bewegung mehr ausgeführt werden
    analogWrite(ENA, 0); // Motor A Bremsen
    analogWrite(ENB, 0); // Motor B Bremsen
  }
}


// PID Werte Berechnung
void myPID()
{
  
  kp = analogRead(A0)*0.1;  Serial.print("kp= ");Serial.print(kp);          //Einlesen des kp Wertes über Poti wählbar
  ki = analogRead(A1)*0.001;  Serial.print(" /// ki= ");Serial.print(ki);   //Einlesen des ki Wertes über Poti wählbar
  kd = analogRead(A2)*1.5;  Serial.print(" /// kd= ");Serial.println(kd);   //Einlesen des kd Wertes über Poti wählbar
  
  // Wahlweise können auch feste Werte genutzt werden
  //kp = 35;  Serial.print("  kp=");Serial.print(kp);
  //kd = 30;  Serial.print("  kd=");Serial.print(kd);
  //ki = 0;  Serial.print("  ki=");Serial.println(ki);
   
  unsigned long now = millis();         //aktuelle Zeit
  float dt = (now - preTime) / 1000.0;  //vergangene Zeit seit vor der letzten Berechnung
  preTime = now;                        //Zeit zu Beginn
  float K = 0.8;
  float A = K / (K + dt);
  f_angle = A * (f_angle + omega * dt) + (1 - A) * r_angle;  Serial.print("  f_angle=");Serial.println(f_angle);  //Winkel Berechnung
  
  timeChange = (now - lastTime);        //vergangene Zeit seit Beginn der letzten Berechnung
  if(timeChange >= SampleTime){
    error = f_angle; // Abweichung von 0 Grad
    errSum += error * timeChange;
    dErr = (error - lastErr) / timeChange;
    Output = kp * error + ki * errSum + kd * dErr;  Serial.print("  Output=");Serial.println(Output);
    lastErr = error;
    lastTime = now;                     //Zeitstempel nach Berechnung
  }
}

void PWMControl(){
  if(Output > 0){            //Vorwärts
    digitalWrite(TN1, HIGH);
    digitalWrite(TN2, LOW);
    digitalWrite(TN3, HIGH);
    digitalWrite(TN4, LOW);
  }
  else if(Output < 0){       //Rückwärts
    digitalWrite(TN1, LOW);
    digitalWrite(TN2, HIGH);
    digitalWrite(TN3, LOW);
    digitalWrite(TN4, HIGH);
  }
  else{                           //Bremse
    digitalWrite(TN1, HIGH);
    digitalWrite(TN2, HIGH);
    digitalWrite(TN3, HIGH);
    digitalWrite(TN4, HIGH);
  }

    analogWrite(ENA, min(255, abs(Output) + Motor_offset)); //Geschwindigkeit rechts
    analogWrite(ENB, min(255, abs(Output) + Motor_offset)); //Geschwindigkeit links
}

Regler Auslegung

Text...

Fazit

Text...

Erfüllte Ziele

Text...

Offene Aufgaben

Text...

---

→ zurück zum Hauptartikel: Fachpraktikum Elektrotechnik (WS 15/16)