Linienverfolgung mit USB-Kamera und PWM-gesteuerten Motoren
(EN google-translate)
(PL google-translate)
Als erstes autonomes System soll der Jetson-Nano-Bot so gestaltet werden, dass er eine Linie auf dem Boden verfolgen kann. Dies soll mit Hilfe einer USB-Kamera und PWM-gesteuerten Motoren geschehen.
Processing-Projekt: Schwerpunkt des Rotanteils in einem Kamerabild erkennen und einzeichnen
Ein noch anzupassender Processing-Sketch wurde für die Elektrokutsche entwickelt und soll zunächst einmal auf den Jetson-Nano übertragen und getestet werden:
AVkamera4nurcam.zip
import processing.video.*;
Capture cam;
Kamera kamera;
public void setup()
{
cam = new Capture(this, 640, 480);
kamera = new Kamera(cam);
size(640,480);
frameRate(30);
}
public void draw()
{
background(0);
kamera.frischeBildAuf();
int xsp = kamera.berechneSchwerpunkt();
kamera.zeichneKamerabild(0,0);
kamera.zeichneSchwerpunkt(0,0,xsp);
stroke(0,0,255);
line(width/2,0,width/2,height);
}
Code 0-1: Hauptprogramm von AVkamera4nurcam.
import processing.video.*;
public class Kamera
{
Capture cam;
public int VGABREITE = 640;
int VGAHOEHE = 480;
Kamera(Capture cam)
{
this.cam = cam;
cam.start();
}
public void frischeBildAuf()
{
if (cam!=null && cam.available() == true)
{
cam.read();
cam.updatePixels();
}
}
public int berechneSchwerpunkt()
{
if(cam==null)
{
return width/2;
}
int[] pix = cam.pixels;
double zaehler = 0.0;
double nenner = 0.0;
for(int y=0;y<VGAHOEHE;y++)
{
for(int x =0; x<VGABREITE;x++)
{
int index = y*VGABREITE + x;
int farbe = pix[index];
int rot = (int)red(farbe);
int gruen = (int)green(farbe);
int blau = (int)blue(farbe);
int echtesrot = rot - gruen - blau;
if(echtesrot<0)
echtesrot = 0;
nenner += (double)echtesrot;
zaehler += (double)x*(double)echtesrot;
}
}
double xsp = zaehler / nenner;
return (int)xsp;
}
public void zeichneKamerabild(int xoff, int yoff)
{
if(cam!=null)
image(cam, xoff, yoff);
}
public void zeichneSchwerpunkt(int xoff, int yoff, int xsp)
{
if(cam==null)
return;
stroke(255);
strokeWeight(4);
line(xsp+xoff,yoff,xsp+xoff,yoff+VGAHOEHE);
}
}//ende Klasse Kamera
Code 0-2: Die Klasse Kamera in AVkamera4nurcam.
Bild 0-1: Der rote Griff des Schraubenziehers wird in diesem Test erkannt. Dort liegt dann der Schwerpunkt des Rotanteils entlang der x-Achse (weiß eingezeichnete Linie).
Übung 1
a) Übertragen Sie den Sketch auf den Jetson Nano und bringen Sie diesen dort zum laufen.
b) Erweitern Sie das Programm so, dass es anzeigt, wie die Motoren einer Abweichung der Linie von der Mitte gegensteuern würden.
AVkamera4nurcam_copy.zip -- studentische Lösung.
Elektronik-Projekt: Motortreiber L293D zur Motoransteuerung verwenden
Der hier zu sehende Schaltplan zeigt, wie der Motortreiber L293D dazu verwendet werden kann zwei Motore anzusteuern:
22_Universal/04_Sensoreingaenge
Bild 0-2: Verwendung des Motortreibers L293D zur Ansteuerung zweier Elektromotoren.
Pin Nr. Bezeichnung Bedeutung 1 En1 PWM-Eingang für 1. Motor, oder auf +5Volt setzen. 2 In1 Richtungswahl 1. Motor In1=5Volt UND In2=0Volt, ODER In1=0Volt UND In2=5Volt 3 Out1 rote Leitung Motor 1 4 GND Masse, Null Volt, Ableiten von Wärme. 5 GND 6 Out2 schwarze Leitung Motor 1 7 In2 siehe In1 8 Vs Leistungsspannungsquelle +Pol, 5..12Volt 9 En2 PWM-Eingang für 2. Motor, oder auf +5Volt setzen. 10 In3 Richtungswahl 2. Motor 11 Out3 rote Leitung Motor 2 12 GND Massen beider Spannungsquellen müssen hier verbunden sein 13 GND 14 Out4 schwarze Leitung Motor 2 15 In4 Richtungswahl 2. Motor 16 Vss Versorgungsspannung für Logikkreis +5Volt
Code 0-3: Übersicht zu den eingezeichneten Anschlüssen.
Bitte beachten Sie die Pinzuordnungen bei folgender Schaltung zwischen Jetson und Platine!
Bild 0-3: Motortreiber-Platine mit IC L293D auf Kupfer-Platine mit Dreierketten.
Übung 2
|
HINWEIS dazu: Da der Jetson keinen 5V-Regler hat, ist die Strombegrenzung an Pin 2 und 4 die gleiche wie die Strombegrenzung des Netzteils, abzüglich des Stroms, der vom Jetson verbraucht wird.
|
https://jetsonhacks.com/nvidia-jetson-nano-j41-header-pinout/
Projekt mit JNI: Ansteuerung des Motortreibers über die GPIO-Pins
Pin auf L293D | GPIO beim Jetson-Nano 1 En1 | gpio200 Pin 31 2 In1 | gpio76 Pin 35 7 In2 | gpio12 Pin 37 9 En2 | gpio168 Pin 32 10 In3 | gpio51 Pin 36 15 In4 | gpio77 Pin 38
Code 0-4: Pinzuordnung
Übung 3 Teil 1
Schreiben Sie Skripte, die über die GPIOs verschiedene Fahrzustände bewirken:
|
Orientieren Sie sich dabei an das LED-Beispiel auf gpio13:
84_Jetson/05_GPIO_und_JNI
Übung 3 Teil 2
Entwickeln Sie mit Hilfe von JNI nun ein Java-Programm, das die Motoren in der zuvor geforderten Art und Weise ansprechen kann.
Übung 3 Teil 3
Anstatt En1 und En2 mit konstanten Spannungen zu belegen, soll auf den entsprechenden GPIO-Pins nun jeweils ein PWM-Signal erzeugt werden. Dazu werden die Pins über ein Java-Programm zyklisch ein- und ausgeschaltet. Zu PWM-Signalen siehe auch:
40_Mikrocontroller/04_PWM/01_Prinzip
Die zyklische Ansteuerung soll über ein Java-Thread erfolgen. Siehe hierzu:
77_Android/04_Threads
Gesamtsystem: Motoransteuerung abhängig von Lage der mit der Kamera erfaßten roten Linie
Schließlich soll die Ansteuerung der Motore abhängig von der im Kamerabild erfassten roten Linie erfolgen.
|
Umsetzung
Alle nachfolgend entwickelten Dateien sind in dem folgenden .zip-Ordner zusammengestellt:
jetson_linienverfolgung.zip
In einem ersten Versuch wird das Kamera-Programm mit einem JNI-Programm kombiniert, das in der Lage ist, die Motoren anzusteuern. Die Schritte dahin werden nachfolgend vorgestellt:
Schritt 1: Herstellen und Anschließen der Motortreiberschaltung
Bild 0-4: Probeaufbau mit USB-Kamera und an hinterer Leiste aufgesteckter Motortreiber-Platine mit angeschlossenen Motoren.
Schritt 2: Test des Motortreibers mittels Temrinal-Skripten
Es gibt folgende Zuordnung: MOTOR LINKS EN1 gpio77 IN1 gpio78 IN2 gpio13 MOTOR RECHTS EN2 gpio19 IN3 gpio20 IN4 gpio51 Tests: EN1 IN1 IN2 0 0 0 Links stop 1 1 0 Links vor 1 0 1 Links rück EN2 IN3 IN4 0 0 0 Rechts stop 1 1 0 Rechts vor 1 0 1 Rechts rück linksstop echo 77 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio77/direction echo 0 > /sys/class/gpio/gpio77/value echo 78 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio78/direction echo 0 > /sys/class/gpio/gpio78/value echo 13 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio13/direction echo 0 > /sys/class/gpio/gpio13/value linksvor echo 77 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio77/direction echo 1 > /sys/class/gpio/gpio77/value echo 78 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio78/direction echo 1 > /sys/class/gpio/gpio78/value echo 13 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio13/direction echo 0 > /sys/class/gpio/gpio13/value linksrueck echo 77 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio77/direction echo 1 > /sys/class/gpio/gpio77/value echo 78 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio78/direction echo 0 > /sys/class/gpio/gpio78/value echo 13 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio13/direction echo 1 > /sys/class/gpio/gpio13/value rechtsstop echo 19 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio19/direction echo 0 > /sys/class/gpio/gpio19/value echo 20 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio20/direction echo 0 > /sys/class/gpio/gpio20/value echo 51 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio51/direction echo 0 > /sys/class/gpio/gpio51/value rechtsvor echo 19 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio19/direction echo 1 > /sys/class/gpio/gpio19/value echo 20 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio20/direction echo 0 > /sys/class/gpio/gpio20/value echo 51 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio51/direction echo 1 > /sys/class/gpio/gpio51/value rechtsrueck echo 19 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio19/direction echo 1 > /sys/class/gpio/gpio19/value echo 20 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio20/direction echo 1 > /sys/class/gpio/gpio20/value echo 51 > /sys/class/gpio/export echo out > /sys/class/gpio/gpio51/direction echo 0 > /sys/class/gpio/gpio51/value
Code 0-5: Inhalte der Terminal-Skripte
Schritt 3: Skripte durch JNI-Funktionen ersetzen
Der Ablauf wird hier sehr knapp dargestellt, weil er dem vorangehenden Projekt zur Ansteuerung einer LED entspricht:
cd jetson_linienverfolgung/02_jni javac jetson/JetsonMotor.java javah -jni -o ./jetson/jetsonmotor.h jetson.JetsonMotor cd jetson gcc -o jetsonmotor.so -shared -Wl,-soname,jetsonmotor.so -I/usr/lib/jvm/java-8-openjdk-arm64/include/ -I/usr/lib/jvm/java-8-openjdk-arm64/include/linux/ jetsonmotor.c -lc sudo cp ./jetsonmotor.so /opt sudo chmod +x /opt/jetsonmotor.so neu anlegen: fuer_jar_datei/jetson/JetsonMotor.class cd .. cd fuer_jar_datei jar cvf jetson.jar * ..in vorhandener library die neue .jar Datei ersetzen. Jetzt Testprogramm zur Motoransteuerung schreiben: cd ~ ./processing-4.0b6-linux-arm64/processing-4.0b6/processing Die Abarbeitung eines Motorbefehls duert knapp etwas unter 100ms also sind pwm-Zyklen hier sehr schwer realisierbar, da zu langsam
Code 0-6: Auszuführende Handlungen
package jetson;
public class JetsonMotor
{
static
{
System.load("/opt/jetsonmotor.so");
}
public static native void linksstop();
public static native void linksvor();
public static native void linksrueck();
public static native void rechtsstop();
public static native void rechtsvor();
public static native void rechtsrueck();
}
Code 0-7: JetsonMotor.java
/* DO NOT EDIT THIS FILE - it is machine generated */
#include <jni.h>
/* Header for class jetson_JetsonMotor */
#ifndef _Included_jetson_JetsonMotor
#define _Included_jetson_JetsonMotor
#ifdef __cplusplus
extern "C" {
#endif
/*
* Class: jetson_JetsonMotor
* Method: linksstop
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksstop
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: linksvor
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksvor
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: linksrueck
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksrueck
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: rechtsstop
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsstop
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: rechtsvor
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsvor
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: rechtsrueck
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsrueck
(JNIEnv *, jclass);
#ifdef __cplusplus
}
#endif
#endif
Code 0-8: jetsonmotor.h
#include <jni.h>
#include <stdio.h>
#include <stdlib.h>
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksstop(JNIEnv *env, jclass clazz)
{
system("echo 77 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio77/direction");
system("echo 0 > /sys/class/gpio/gpio77/value");
system("echo 78 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio78/direction");
system("echo 0 > /sys/class/gpio/gpio78/value");
system("echo 13 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio13/direction");
system("echo 0 > /sys/class/gpio/gpio13/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksvor(JNIEnv *env, jclass clazz)
{
system("echo 77 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio77/direction");
system("echo 1 > /sys/class/gpio/gpio77/value");
system("echo 78 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio78/direction");
system("echo 1 > /sys/class/gpio/gpio78/value");
system("echo 13 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio13/direction");
system("echo 0 > /sys/class/gpio/gpio13/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksrueck(JNIEnv *env, jclass clazz)
{
system("echo 77 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio77/direction");
system("echo 1 > /sys/class/gpio/gpio77/value");
system("echo 78 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio78/direction");
system("echo 0 > /sys/class/gpio/gpio78/value");
system("echo 13 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio13/direction");
system("echo 1 > /sys/class/gpio/gpio13/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsstop(JNIEnv *env, jclass clazz)
{
system("echo 19 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio19/direction");
system("echo 0 > /sys/class/gpio/gpio19/value");
system("echo 20 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio20/direction");
system("echo 0 > /sys/class/gpio/gpio20/value");
system("echo 51 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio51/direction");
system("echo 0 > /sys/class/gpio/gpio51/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsvor(JNIEnv *env, jclass clazz)
{
system("echo 19 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio19/direction");
system("echo 1 > /sys/class/gpio/gpio19/value");
system("echo 20 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio20/direction");
system("echo 0 > /sys/class/gpio/gpio20/value");
system("echo 51 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio51/direction");
system("echo 1 > /sys/class/gpio/gpio51/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsrueck(JNIEnv *env, jclass clazz)
{
system("echo 19 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio19/direction");
system("echo 1 > /sys/class/gpio/gpio19/value");
system("echo 20 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio20/direction");
system("echo 1 > /sys/class/gpio/gpio20/value");
system("echo 51 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio51/direction");
system("echo 0 > /sys/class/gpio/gpio51/value");
}
Code 0-9: jetsonmotor.c
Schritt 4: Java-Testprogramme schreiben
import jetson.*;
public void setup()
{
frameRate(1);
}
int x=0;
public void draw()
{
switch(x/3)
{
case 0:
println("LINKS VOR");
JetsonMotor.linksvor();
break;
case 1:
println("LINKS RÜCK");
JetsonMotor.linksrueck();
break;
case 2:
println("LINKS STOP");
JetsonMotor.linksstop();
break;
case 3:
println("RECHTS VOR");
JetsonMotor.rechtsvor();
break;
case 4:
println("RECHTS RUECK");
JetsonMotor.rechtsrueck();
break;
case 5:
println("RECHTS STOP");
JetsonMotor.rechtsstop();
break;
default:
break;
}
x++;
}
Code 0-10: TestJetsonMotor.pde
import jetson.*;
public void setup()
{
println("Messung der Abarbeitungsdauer eines Befehls:");
long T = System.currentTimeMillis();
int anzahl = 100;
for(int i=0;i<anzahl;i++)
JetsonMotor.linksvor();
long diff = System.currentTimeMillis() - T;
double millisekunden = (double)diff/(double)anzahl;
println("Dauer für die Abarbeitung eines einzelnen Motorbefehls: "+millisekunden+" Millisekunden");
JetsonMotor.linksstop();
frameRate(1);
}
public void draw()
{
}
Code 0-11: TestJetsonMotor_Dauer.pde -- Ermitteln der Abarbeitungsdauer der Motorbefehle
import processing.video.*;
import jetson.*;
Capture cam;
Kamera kamera;
public void setup()
{
cam = new Capture(this, 640, 480);
kamera = new Kamera(cam);
size(640,480);
frameRate(30);
}
int x=0;
public void draw()
{
background(0);
//kamera.drawTestpixel();
kamera.frischeBildAuf();
int xsp = kamera.berechneSchwerpunkt();
kamera.zeichneKamerabild(0,0);
kamera.zeichneSchwerpunkt(0,0,xsp);
stroke(0,0,255);
line(width/2,0,width/2,height);
//Motoren über Lage des Schwerpunkts ansteuern:
if(xsp>width/4 && xsp<width/2+width/4)
{
if(xsp>width/2-width/10 && xsp<width/2 + width/10)
{
JetsonMotor.linksvor();
JetsonMotor.rechtsvor();
}
else if(xsp<=width/2-width/10)
{
if(x%2==0)
JetsonMotor.linksstop();
else
JetsonMotor.linksvor();
JetsonMotor.rechtsvor();
}
else
{
JetsonMotor.linksvor();
if(x%2==0)
JetsonMotor.rechtsstop();
else
JetsonMotor.rechtsvor();
}
}
else
{
JetsonMotor.linksstop();
JetsonMotor.rechtsstop();
}
x++;
}
Code 0-12: JetsonMotor_Kamera.pde -- Ansteuern der Motoren abhängig von der Lage der roten Linie.
import processing.video.*;
public class Kamera
{
Capture cam;
public int VGABREITE = 640;
int VGAHOEHE = 480;
Kamera(Capture cam)
{
this.cam = cam;
cam.start();
}
public void frischeBildAuf()
{
if (cam!=null && cam.available() == true)
{
cam.read();
cam.updatePixels();
}
}
public int berechneSchwerpunkt()
{
if(cam==null)
{
return width/2;
}
int[] pix = cam.pixels;
double zaehler = 0.0;
double nenner = 0.0;
for(int y=0;y<VGAHOEHE;y++)
{
for(int x =0; x<VGABREITE;x++)
{
int index = y*VGABREITE + x;
int farbe = pix[index];
int rot = (int)red(farbe);
int gruen = (int)green(farbe);
int blau = (int)blue(farbe);
int echtesrot = rot - gruen - blau;
if(echtesrot<0)
echtesrot = 0;
nenner += (double)echtesrot;
zaehler += (double)x*(double)echtesrot;
}
}
double xsp = zaehler / nenner;
return (int)xsp;
}
public void zeichneKamerabild(int xoff, int yoff)
{
if(cam!=null)
image(cam, xoff, yoff);
}
public void zeichneSchwerpunkt(int xoff, int yoff, int xsp)
{
if(cam==null)
return;
stroke(255);
strokeWeight(4);
line(xsp+xoff,yoff,xsp+xoff,yoff+VGAHOEHE);
}
}//ende Klasse Kamera
Code 0-13: Kamera -Klasse im gleichen Projekt.
Graduell verbesserte Version zur Erzeugung von PWM-Signalen mit Kamera-basiertem Beispiel für Linienverfolgung
pwmjniverbessert.zip
cd jetson_linienverfolgung/04_jnineu javac jetson/JetsonMotor.java javah -jni -o ./jetson/jetsonmotor.h jetson.JetsonMotor cd jetson gcc -o jetsonmotor.so -shared -Wl,-soname,jetsonmotor.so -I/usr/lib/jvm/java-8-openjdk-arm64/include/ -I/usr/lib/jvm/java-8-openjdk-arm64/include/linux/ jetsonmotor.c -lc sudo cp ./jetsonmotor.so /opt sudo chmod +x /opt/jetsonmotor.so neu anlegen: fuer_jar_datei/jetson/JetsonMotor.class cd .. cd fuer_jar_datei jar cvf jetson.jar * ..in vorhandener library die neue .jar Datei ersetzen. Jetzt Testprogramm zur Motoransteuerung schreiben: cd ~ ./processing-4.0b6-linux-arm64/processing-4.0b6/processing Die Abarbeitung eines Motorbefehls dauert JETZT knapp etwas unter 10ms
Code 0-14: Ablauf zu Herstellung, Ersetzung und Test der JNI-Library
package jetson;
public class JetsonMotor
{
static
{
System.load("/opt/jetsonmotor.so");
}
public static native void init(); //NEU
public static native void enablelinks(); //NEU
public static native void disablelinks(); //NEU
public static native void enablerechts(); //NEU
public static native void disablerechts(); //NEU
public static native void linksstop();
public static native void linksvor();
public static native void linksrueck();
public static native void rechtsstop();
public static native void rechtsvor();
public static native void rechtsrueck();
}
Code 0-15: jetson/JetsonMotor.java -- Neue Java-Klasse zur Ansteuerung der GPIO
/* DO NOT EDIT THIS FILE - it is machine generated */
#include <jni.h>
/* Header for class jetson_JetsonMotor */
#ifndef _Included_jetson_JetsonMotor
#define _Included_jetson_JetsonMotor
#ifdef __cplusplus
extern "C" {
#endif
/*
* Class: jetson_JetsonMotor
* Method: init
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_init
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: enablelinks
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_enablelinks
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: disablelinks
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_disablelinks
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: enablerechts
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_enablerechts
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: disablerechts
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_disablerechts
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: linksstop
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksstop
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: linksvor
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksvor
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: linksrueck
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksrueck
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: rechtsstop
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsstop
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: rechtsvor
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsvor
(JNIEnv *, jclass);
/*
* Class: jetson_JetsonMotor
* Method: rechtsrueck
* Signature: ()V
*/
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsrueck
(JNIEnv *, jclass);
#ifdef __cplusplus
}
#endif
#endif
Code 0-16: jetsonmotor.h -- Automatisch erstelltes Header-File
#include <jni.h>
#include <stdio.h>
#include <stdlib.h>
//NEU
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_init(JNIEnv *env, jclass clazz)
{
system("echo 77 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio77/direction");
system("echo 78 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio78/direction");
system("echo 13 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio13/direction");
system("echo 19 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio19/direction");
system("echo 20 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio20/direction");
system("echo 51 > /sys/class/gpio/export");
system("echo out > /sys/class/gpio/gpio51/direction");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_enablelinks(JNIEnv *env, jclass clazz)
{
system("echo 1 > /sys/class/gpio/gpio77/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_disablelinks(JNIEnv *env, jclass clazz)
{
system("echo 0 > /sys/class/gpio/gpio77/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_enablerechts(JNIEnv *env, jclass clazz)
{
system("echo 1 > /sys/class/gpio/gpio19/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_disablerechts(JNIEnv *env, jclass clazz)
{
system("echo 0 > /sys/class/gpio/gpio19/value");
}
//ALT
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksstop(JNIEnv *env, jclass clazz)
{
system("echo 0 > /sys/class/gpio/gpio77/value");
system("echo 0 > /sys/class/gpio/gpio78/value");
system("echo 0 > /sys/class/gpio/gpio13/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksvor(JNIEnv *env, jclass clazz)
{
system("echo 1 > /sys/class/gpio/gpio77/value");
system("echo 1 > /sys/class/gpio/gpio78/value");
system("echo 0 > /sys/class/gpio/gpio13/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_linksrueck(JNIEnv *env, jclass clazz)
{
system("echo 1 > /sys/class/gpio/gpio77/value");
system("echo 0 > /sys/class/gpio/gpio78/value");
system("echo 1 > /sys/class/gpio/gpio13/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsstop(JNIEnv *env, jclass clazz)
{
system("echo 0 > /sys/class/gpio/gpio19/value");
system("echo 0 > /sys/class/gpio/gpio20/value");
system("echo 0 > /sys/class/gpio/gpio51/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsvor(JNIEnv *env, jclass clazz)
{
system("echo 1 > /sys/class/gpio/gpio19/value");
system("echo 0 > /sys/class/gpio/gpio20/value");
system("echo 1 > /sys/class/gpio/gpio51/value");
}
JNIEXPORT void JNICALL Java_jetson_JetsonMotor_rechtsrueck(JNIEnv *env, jclass clazz)
{
system("echo 1 > /sys/class/gpio/gpio19/value");
system("echo 1 > /sys/class/gpio/gpio20/value");
system("echo 0 > /sys/class/gpio/gpio51/value");
}
Code 0-17: jetsonmotor.c -- unter Verwendung des Header-Files implementierte C-Funktionen. init() muß zuerst einmal aufgerufen werden!
PWM-Erzeugung in den Test-Sketchen in Processing
public class PWM
{
int plinks;
int prechts;
int plinksabs;
int prechtsabs;
int STUFEN;
PWMlinks pwmlinks;
PWMrechts pwmrechts;
public PWM(int STUFEN)
{
this.STUFEN = STUFEN;
plinks=0;
prechts=0;
//NEU: (nicht vergessen!)
JetsonMotor.init();
JetsonMotor.linksvor();
JetsonMotor.rechtsvor();
JetsonMotor.disablelinks();
JetsonMotor.disablerechts();
pwmlinks = new PWMlinks(this);
pwmrechts = new PWMrechts(this);
}
public void pwmLinks(int x)
{
if(x>=0 && plinks<0)
JetsonMotor.linksvor();
else if(x<0 && plinks>=0)
JetsonMotor.linksrueck();
plinks = x;
if(x>=0)
plinksabs = x;
else
plinksabs = -x;
}
public void pwmRechts(int x)
{
if(x>=0 && prechts<0)
JetsonMotor.rechtsvor();
else if(x<0 && prechts>=0)
JetsonMotor.rechtsrueck();
prechts = x;
if(x>=0)
prechtsabs = x;
else
prechtsabs = -x;
}
}
Code 0-18: Klasse PWM
public class PWMlinks implements Runnable
{
private ScheduledExecutorService schedExecService;
PWM pwm;
public PWMlinks(PWM pwm)
{
this.pwm = pwm;
schedExecService = Executors.newSingleThreadScheduledExecutor();
schedExecService.scheduleAtFixedRate(this, 0, 12, TimeUnit.MILLISECONDS);
}
//12 Millisekunden Zyklus.
//5 Geschwindigkeitsstufen => PWM-Periode ist 60 Millisekunden lang
int index=0;
public void run()
{
if(index<pwm.plinksabs)
JetsonMotor.enablelinks();
else
JetsonMotor.disablelinks();
index++;
if(index>pwm.STUFEN)
index=0;
}
}
Code 0-19: Klasse PWMlinks (von PWM benutzt.)
public class PWMrechts implements Runnable
{
private ScheduledExecutorService schedExecService;
PWM pwm;
public PWMrechts(PWM pwm)
{
this.pwm = pwm;
schedExecService = Executors.newSingleThreadScheduledExecutor();
schedExecService.scheduleAtFixedRate(this, 0, 12, TimeUnit.MILLISECONDS);
}
//12 Millisekunden Zyklus.
//5 Geschwindigkeitsstufen => PWM-Periode ist 60 Millisekunden lang
int index=0;
public void run()
{
if(index<pwm.prechtsabs)
JetsonMotor.enablerechts();
else
JetsonMotor.disablerechts();
index++;
if(index>pwm.STUFEN)
index=0;
}
}
Code 0-20: Klasse PWMrechts (von PWM benutzt)
import processing.video.*;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.TimeUnit;
import jetson.*;
Capture cam;
Kamera kamera;
PWM pwm;
int STUFEN = 8;
float anteilvorz=0.0f;
public void setup()
{
cam = new Capture(this, 640, 480);
kamera = new Kamera(cam);
pwm = new PWM(STUFEN);
size(640,480);
frameRate(30);
}
int x=0;
public void draw()
{
background(0);
//kamera.drawTestpixel();
kamera.frischeBildAuf();
int xsp = kamera.berechneSchwerpunkt();
kamera.zeichneKamerabild(0,0);
kamera.zeichneSchwerpunkt(0,0,xsp);
stroke(0,0,255);
line(width/2,0,width/2,height);
//Aktivität der Motoren und Regelabweichung anzeigen:
textSize(24.0f);
fill(255);
text("Regeldifferenz = "+anteilvorz,30,30);
fill(255,0,0);
text("PWM links = "+pwm.plinks,30,90);
fill(0,255,0);
text("PWM rechts = "+pwm.prechts,width-160,90);
//Motoren über Lage des Schwerpunkts ansteuern:
if(xsp>width/4 && xsp<width/2+width/4)
{
float anteil = (xsp-width/4.0)/(width/2.0); // 0..1
anteilvorz = (anteil-0.5)*2.0; // -1..+1
if(anteilvorz==0.0)
{
pwm.pwmLinks(STUFEN);
pwm.pwmRechts(STUFEN);
}
else if(anteilvorz>0.0)
{
float u = (float)STUFEN - (float)STUFEN*anteilvorz;
pwm.pwmLinks(STUFEN);
pwm.pwmRechts((int)round(u));
}
else //if(anteilvorz<0.0)
{
float u = (float)STUFEN - (float)STUFEN*(-anteilvorz);
pwm.pwmLinks((int)round(u));
pwm.pwmRechts(STUFEN);
}
}
else
{
pwm.pwmLinks(0);
pwm.pwmRechts(0);
}
x++;
}
Code 0-21: Hauptprogramm von TestneuJetsonMotor_Kamera