3.1.6 Bewegungssteuerung¶

Einführung¶

In dieser Lektion werden wir ein einfaches Bewegungserfassungs- und Steuergerät herstellen. Die MPU6050 wird als Sensor und der Schrittmotor als gesteuertes Gerät verwendet. Mit der am Handschuh montierten MPU6050 können Sie den Schrittmotor durch Drehen Ihres Handgelenks steuern.

Komponenten¶

Schematische Darstellung¶

T-Karte Name	physisch	wiringPi	BCM
GPIO18	Pin 12	1	18
GPIO23	Pin 16	4	23
GPIO24	Pin 18	5	24
GPIO25	Pin 22	6	25
SDA1	Pin 3
SCL1	Pin 5

Experimentelle Verfahren¶

Schritt 1: Bauen Sie die Schaltung auf.

Für Benutzer in C-Sprache¶

Schritt 2: Gehen Sie zum Ordner der Kode.

cd /home/pi/davinci-kit-for-raspberry-pi/c/3.1.6/

Schritt 3: Kompilieren Sie die Kode.

gcc 3.1.6_MotionControl.c -lwiringPi -lm

Schritt 4: Führen Sie die ausführbare Datei aus.

sudo ./a.out

Wenn der Neigungswinkel von mpu6050 auf der Y-Achse größer als 45 ℃, ist, dreht sich der Schrittmotor während der Kode gegen den Uhrzeigersinn. Bei weniger als -45 ℃, dreht sich der Schrittmotor im Uhrzeigersinn.

Code Erklärung

double mpu6050(){
    acclX = read_word_2c(0x3B);
    acclY = read_word_2c(0x3D);
    acclZ = read_word_2c(0x3F);
    acclX_scaled = acclX / 16384.0;
    acclY_scaled = acclY / 16384.0;
    acclZ_scaled = acclZ / 16384.0;
    double angle=get_y_rotation(acclX_scaled, acclY_scaled, acclZ_scaled);
    return angle;
}

mpu6050 erhält den Neigungswinkel in Richtung der Y-Achse.

void rotary(char direction){
    if(direction == 'c'){
        for(int j=0;j<4;j++){
            for(int i=0;i<4;i++)
                {digitalWrite(motorPin[i],0x99>>j & (0x08>>i));}
            delayMicroseconds(stepSpeed);
        }
    }
    else if(direction =='a'){
        for(int j=0;j<4;j++){
            for(int i=0;i<4;i++)
                {digitalWrite(motorPin[i],0x99<<j & (0x80>>i));}
            delayMicroseconds(stepSpeed);
        }
    }
}

Wenn die empfangene Richtung Taste ‚c‘ ist, dreht sich der Schrittmotor im Uhrzeigersinn; wenn die Taste ‚a‘ ist, dreht der Motor gegen den Uhrzeigersinn. Siehe 1.3.3 Schrittmotor für weitere Details zur Berechnung der Drehrichtung des Schrittmotors.

int main()
{
    setup();
    double angle;
    while(1) {
        angle = mpu6050();
        if (angle >=45){rotary('a');}
        else if (angle<=-45){rotary('c');}
    }
    return 0;
}

Der Neigungswinkel in Richtung der Y-Achse wird von mpu6050 abgelesen. Wenn er größer als 45 ℃, ist, dreht sich der Schrittmotor gegen den Uhrzeigersinn. Bei weniger als -45 ℃, dreht sich der Schrittmotor im Uhrzeigersinn.

Für Python-Sprachbenutzer¶

Schritt 2: Gehen Sie zum Ordner der Kode.

cd /home/pi/davinci-kit-for-raspberry-pi/python/

Schritt 3: Führen Sie die ausführbare Datei aus.

sudo python3 3.1.6_MotionControl.py

Wenn der Neigungswinkel von mpu6050 auf der Y-Achse größer als 45 ℃, ist, dreht sich der Schrittmotor während der Kode gegen den Uhrzeigersinn. Bei weniger als -45 ℃, dreht sich der Schrittmotor im Uhrzeigersinn.

Code

Bemerkung

Sie können den folgenden Code Ändern/Zurücksetzen/Kopieren/Ausführen/Stoppen . Zuvor müssen Sie jedoch zu einem Quellcodepfad wie davinci-kit-for-raspberry-pi/python gehen.

import RPi.GPIO as GPIO
import smbus
import math
import time



# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

bus = smbus.SMBus(1)
address = 0x68
bus.write_byte_data(address, power_mgmt_1, 0)

#Stepper motor pins
motorPin = (18,23,24,25)
rolePerMinute =15
stepsPerRevolution = 2048
stepSpeed = (60/rolePerMinute)/stepsPerRevolution

#mpu6050
def read_byte(adr):
    return bus.read_byte_data(address, adr)

def read_word(adr):
    high = bus.read_byte_data(address, adr)
    low = bus.read_byte_data(address, adr+1)
    val = (high << 8) + low
    return val

def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val

def dist(a,b):
    return math.sqrt((a*a)+(b*b))

def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)

def mpu6050():
    accel_xout = read_word_2c(0x3b)
    accel_yout = read_word_2c(0x3d)
    accel_zout = read_word_2c(0x3f)
    accel_xout_scaled = accel_xout / 16384.0
    accel_yout_scaled = accel_yout / 16384.0
    accel_zout_scaled = accel_zout / 16384.0
    angle=get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
    return angle

#Stepper Motor
def rotary(direction):
    if(direction == 'c'):
        for j in range(4):
            for i in range(4):
                GPIO.output(motorPin[i],0x99>>j & (0x08>>i))
            time.sleep(stepSpeed)

    elif(direction == 'a'):
        for j in range(4):
            for i in range(4):
                GPIO.output(motorPin[i],0x99<<j & (0x80>>i))
            time.sleep(stepSpeed)


def setup():
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BCM)
    for i in motorPin:
        GPIO.setup(i, GPIO.OUT)


def loop():
    while True:
        angle=mpu6050()
        if angle >=45 :
            rotary('a')
        elif angle <=-45:
            rotary('c')

def destroy():
    GPIO.cleanup()

if __name__ == '__main__':
    setup()
    try:
        loop()
    except KeyboardInterrupt:
        destroy()