From 888e2d207e2d99b68a5f5cc31450b3f47708509b Mon Sep 17 00:00:00 2001 From: Natanael Adityasatria Date: Wed, 14 Oct 2015 19:16:09 +0200 Subject: [PATCH] ev3 stays inside black line --- natanael/ev3/Main.java | 128 +++++++++++++++++++++++++++++++++++++++++ natanael/ex11/Makefile | 99 +++++++++++++++++++++++++++++++ natanael/ex11/ex11a.c | 60 +++++++++++++++++++ 3 files changed, 287 insertions(+) create mode 100644 natanael/ev3/Main.java create mode 100644 natanael/ex11/Makefile create mode 100644 natanael/ex11/ex11a.c diff --git a/natanael/ev3/Main.java b/natanael/ev3/Main.java new file mode 100644 index 0000000..1b23b5e --- /dev/null +++ b/natanael/ev3/Main.java @@ -0,0 +1,128 @@ +import lejos.hardware.lcd.LCD; +import lejos.hardware.motor.EV3LargeRegulatedMotor; +import lejos.hardware.port.MotorPort; +import lejos.hardware.sensor.EV3TouchSensor; +import lejos.hardware.sensor.EV3UltrasonicSensor; +import lejos.hardware.sensor.NXTLightSensor; +import lejos.robotics.SampleProvider; +import lejos.utility.Delay; + +import java.util.Random; + +import lejos.hardware.*; +import lejos.hardware.ev3.LocalEV3; + +public class Main { + + static float BLACK_LINE = (float) 0.5; + static float SOMETHING_IN_FRONT = (float) 0.24; + static float SOMETHING_IN_LEFT = 1; + static float SOMETHING_IN_RIGHT = 1; + static int MINIMUM_TURN = 120; + static int MAXIMUM_TURN = 180; + + static EV3LargeRegulatedMotor leftMotor, rightMotor; + static EV3TouchSensor touchLeftSensor, touchRightSensor; + static NXTLightSensor lightSensor; + static EV3UltrasonicSensor distanceSensor; + + private static Random rnd = new Random(); + private static long StartTime = 0; + + public static void main(String[] args) { + leftMotor = new EV3LargeRegulatedMotor(MotorPort.A); + rightMotor = new EV3LargeRegulatedMotor(MotorPort.D); + + touchLeftSensor = new EV3TouchSensor(LocalEV3.get().getPort("S1")); + touchRightSensor = new EV3TouchSensor(LocalEV3.get().getPort("S4")); + lightSensor = new NXTLightSensor(LocalEV3.get().getPort("S2")); + distanceSensor = new EV3UltrasonicSensor(LocalEV3.get().getPort("S3")); + + SampleProvider touchLeft = touchLeftSensor.getTouchMode(), touchRight = touchRightSensor.getTouchMode(), + light = lightSensor.getRedMode(), distance = distanceSensor.getDistanceMode(); + + float[] touchLeftSamples = new float[touchLeft.sampleSize()], + touchRightSamples = new float[touchRight.sampleSize()], lightSamples = new float[light.sampleSize()], + distanceSamples = new float[distance.sampleSize()]; + + // Main Looping + while (!Button.ESCAPE.isDown()) { + StartTime = System.currentTimeMillis(); + + touchLeft.fetchSample(touchLeftSamples, 0); + touchRight.fetchSample(touchRightSamples, 0); + light.fetchSample(lightSamples, 0); + distance.fetchSample(distanceSamples, 0); + + LCD.drawString("TL: " + touchLeftSamples[0], 0, 0); + LCD.drawString("TR: " + touchRightSamples[0], 0, 1); + LCD.drawString("LI: " + lightSamples[0], 0, 2); + LCD.drawString("DI: " + distanceSamples[0], 0, 3); // distance in + // meter + + if (lightSamples[0] < BLACK_LINE) { + LCD.drawString("BLACK", 0, 4); + Stop(); + if (rnd.nextBoolean()) { + TurnLeft(GetRandomTurn()); + } else { + TurnRight(GetRandomTurn()); + } + } else { + LCD.drawString("WHITE", 0, 4); + GoForward(); + } + + if (distanceSamples[0] < SOMETHING_IN_FRONT) { + LCD.drawString("STOP", 0, 5); + } else { + LCD.drawString("GO ", 0, 5); + } + + LCD.drawString("T: " + (System.currentTimeMillis() - StartTime), 0, 6); + + if (System.currentTimeMillis() - StartTime < 450) { + + } + } + } + + public static int GetRandomTurn() { + return rnd.nextInt((MAXIMUM_TURN - MINIMUM_TURN) + 1) + MINIMUM_TURN; + } + + public static void GoForward() { + leftMotor.setSpeed(300); + rightMotor.setSpeed(300); + + leftMotor.forward(); + rightMotor.forward(); + } + + public static void GoBack() { + leftMotor.setSpeed(300); + rightMotor.setSpeed(300); + + leftMotor.backward(); + rightMotor.backward(); + Delay.msDelay(100); + Stop(); + + } + + public static void Stop() { + leftMotor.stop(true); + rightMotor.stop(); + } + + public static void TurnRight(int degree) { + leftMotor.rotate(degree, true); + rightMotor.rotate(-degree); + } + + public static void TurnLeft(int degree) { + leftMotor.rotate(-degree, true); + rightMotor.rotate(degree); + + } +} \ No newline at end of file diff --git a/natanael/ex11/Makefile b/natanael/ex11/Makefile new file mode 100644 index 0000000..11eada6 --- /dev/null +++ b/natanael/ex11/Makefile @@ -0,0 +1,99 @@ +###### CONFIGURATION ###### + +### List of applications to be build +APPLICATIONS = ex11a + +### Note: to override the search path for the xeno-config script, use "make XENO=..." + + +### List of modules to be build +MODULES = + +### Note: to override the kernel source path, use "make KSRC=..." + + + +###### USER SPACE BUILD (no change required normally) ###### +ifeq ($(KERNELRELEASE),) +ifneq ($(APPLICATIONS),) + +### Default Xenomai installation path +XENO ?= /usr/xenomai + +XENOCONFIG=$(shell PATH=$(XENO):$(XENO)/bin:$(PATH) which xeno-config 2>/dev/null) + +### Sanity check +ifeq ($(XENOCONFIG),) +all:: + @echo ">>> Invoke make like this: \"make XENO=/path/to/xeno-config\" <<<" + @echo +endif + + +CC=$(shell $(XENOCONFIG) --cc) + +CFLAGS=$(shell $(XENOCONFIG) --xeno-cflags) $(MY_CFLAGS) + +LDFLAGS=$(shell $(XENOCONFIG) --xeno-ldflags) $(MY_LDFLAGS) -lnative + +# This includes the library path of given Xenomai into the binary to make live +# easier for beginners if Xenomai's libs are not in any default search path. +LDFLAGS+=-Xlinker -rpath -Xlinker $(shell $(XENOCONFIG) --libdir) + +LDFLAGS+= -lrtdk + +all:: $(APPLICATIONS) + +clean:: + $(RM) $(APPLICATIONS) *.o + +endif +endif + + + +###### SPECIAL TARGET RULES ###### +rtprint: rtprint.c + $(CC) $(CFLAGS) $? $(LDFLAGS) -lrtdk -o $@ + + + +###### KERNEL MODULE BUILD (no change required normally) ###### +ifneq ($(MODULES),) + +### Default to sources of currently running kernel +KSRC ?= /lib/modules/$(shell uname -r)/build + +OBJS := ${patsubst %, %.o, $(MODULES)} +CLEANMOD := ${patsubst %, .%*, $(MODULES)} +PWD := $(shell if [ "$$PWD" != "" ]; then echo $$PWD; else pwd; fi) + +### Kernel 2.6 +ifeq ($(findstring 2.6,$(KSRC)),2.6) + +obj-m := $(OBJS) +EXTRA_CFLAGS := -I$(KSRC)/include/xenomai -I$(KSRC)/include/xenomai/posix $(ADD_CFLAGS) + +all:: + $(MAKE) -C $(KSRC) SUBDIRS=$(PWD) modules + +### Kernel 2.4 +else + +ARCH ?= $(shell uname -i) +INCLUDE := -I$(KSRC)/include/xenomai -I$(KSRC)/include/xenomai/compat -I$(KSRC)/include/xenomai/posix +CFLAGS += $(shell $(MAKE) -s -C $(KSRC) CC=$(CC) ARCH=$(ARCH) SUBDIRS=$(PWD) modules) $(INCLUDE) + +all:: $(OBJS) + +endif + +## Target for capturing 2.4 module CFLAGS +modules: + @echo "$(CFLAGS)" + +clean:: + $(RM) $(CLEANMOD) *.o *.ko *.mod.c Module*.symvers + $(RM) -R .tmp* + +endif \ No newline at end of file diff --git a/natanael/ex11/ex11a.c b/natanael/ex11/ex11a.c new file mode 100644 index 0000000..b24880e --- /dev/null +++ b/natanael/ex11/ex11a.c @@ -0,0 +1,60 @@ +#include +#include +#include +#include +#include +#include +#include + +const RTIME period = 1e5; +const int nsamples = 10000; + +RT_TASK task; +RTIME write_time; +RTIME arr_write_time[10000]; +RTIME time_diff[10000]; + +void do_task(void *arg) +{ + int i; + rt_task_set_periodic(NULL, TM_NOW, period); + + for(i=0; i