aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--nicla/road.py67
-rw-r--r--nicla/signs_detect.py2
-rw-r--r--zumo/control.cpp10
-rw-r--r--zumo/pid.cpp3
-rw-r--r--zumo/protocol.cpp16
-rw-r--r--zumo/protocol.h3
-rw-r--r--zumo/zumo.ino12
7 files changed, 62 insertions, 51 deletions
diff --git a/nicla/road.py b/nicla/road.py
index 1b06e78..143a9c5 100644
--- a/nicla/road.py
+++ b/nicla/road.py
@@ -27,41 +27,40 @@ points = [(STRETCH, HORIZON),
(WIDTH-1+SQUEEZE, HEIGHT-1),
(-SQUEEZE, HEIGHT-1)]
-def drive(driveImg):
- img.to_grayscale()
- img.replace(vflip=True, hmirror=True)
- img.rotation_corr(corners=points)
- img.gaussian(3)
-
- offset_sum = 0.0
- offset_count = 0.0
- for blob in img.find_blobs([(ROAD_MIN_BRIGHTNESS, 0xff)], pixels_threshold=100):
- img.draw_rectangle(blob.rect())
- area_weight = MIN_AREA + min(MAX_AREA, blob.w() * blob.h()) # limit max area_weight so small blobs still have impact
- horizontal_pos = (blob.x() + blob.w()/2) / WIDTH
- offset_sum += horizontal_pos * area_weight
- offset_count += area_weight
- # dit tegen niemand zeggen
- if offset_count < 0.01: return
- avg = offset_sum / offset_count
- avg = avg * 2 - 1
- avg *= STEERING_ENTHOUSIASM
- avg = max(-1, min(1, avg))
-
- print(avg)
- steerByte = int((avg + 1.0) * (DUI_CMD_STEER_END - DUI_CMD_STEER_START) / 2 + DUI_CMD_STEER_START)
- uart.uart_buffer(steerByte)
-
-
-speed = signs_detect.init_kpts("speed")
-stop = signs_detect.init_kpts("stop")
-car = signs_detect.init_kpts("image")
+
+def drive(img):
+ img.to_grayscale()
+ img.replace(vflip=True, hmirror=True)
+ img.rotation_corr(corners=points)
+ img.gaussian(3)
+
+ offset_sum = 0.0
+ offset_count = 0.0
+ for blob in img.find_blobs([(ROAD_MIN_BRIGHTNESS, 0xff)], pixels_threshold=100):
+ img.draw_rectangle(blob.rect())
+ area_weight = MIN_AREA + min(MAX_AREA, blob.w() * blob.h()) # limit max area_weight so small blobs still have impact
+ horizontal_pos = (blob.x() + blob.w()/2) / WIDTH
+ offset_sum += horizontal_pos * area_weight
+ offset_count += area_weight
+ # dit tegen niemand zeggen
+ if offset_count < 0.01: return
+ avg = offset_sum / offset_count
+ avg = avg * 2 - 1
+ avg *= STEERING_ENTHOUSIASM
+ avg = max(-1, min(1, avg))
+
+ steerByte = int((avg + 1.0) * (DUI_CMD_STEER_END - DUI_CMD_STEER_START) / 2 + DUI_CMD_STEER_START)
+
+ uart.uart_buffer(steerByte)
+
+
+
while(True):
- img = sensor.snapshot()
- data = traffic_light.traf_lights(img)
- if data is not None:
- uart.uart_buffer(data)
+ #img = sensor.snapshot()
+ #data = traffic_light.traf_lights(img)
+ #if data is not None:
+ #uart.uart_buffer(data)
sign_img = sensor.snapshot()
@@ -71,4 +70,4 @@ while(True):
drive_img = sensor.snapshot()
drive(drive_img)
- #uart.uart_buffer(DUI_CMD_SPEED_END)
+ uart.uart_buffer(0x1f)
diff --git a/nicla/signs_detect.py b/nicla/signs_detect.py
index 786972d..57b6c3e 100644
--- a/nicla/signs_detect.py
+++ b/nicla/signs_detect.py
@@ -15,7 +15,7 @@ def match_kpts(kpts0, kpts1):
#print("matched:%d dt:%d"%(match.count(), match.theta()))
if match.count() > 0:
print(match.count())
- return match.count() > 1
+ return match.count() > 0
else:
return 0
diff --git a/zumo/control.cpp b/zumo/control.cpp
index 778969a..71e7537 100644
--- a/zumo/control.cpp
+++ b/zumo/control.cpp
@@ -6,11 +6,11 @@
#include "control.h"
#define DUI_SPEED_MOD 96.0f
-#define DUI_MOTOR_DIFF 0.6f
+#define DUI_MOTOR_DIFF 0.62f
void apply_state(dui_state_t *state) {
- float motor_l = 0.5f * state->speed * (+1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD;
- float motor_r = 0.5f * state->speed * (-1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD;
+ float motor_l = 0.5f * state->Speed * (+1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD;
+ float motor_r = 0.5f * state->Speed * (-1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD;
Serial.print(motor_l);
Serial.print(" ");
@@ -18,8 +18,7 @@ void apply_state(dui_state_t *state) {
Serial.println("");
- Zumo32U4Motors::setLeftSpeed((int16_t) motor_l);
- Zumo32U4Motors::setRightSpeed((int16_t) motor_r);
+ Zumo32U4Motors::setSpeeds((int16_t) motor_l,(int16_t) motor_r);
// TODO: print sign on OLED screen
}
@@ -38,6 +37,5 @@ unsigned char uart_read() {
}
delayMicroseconds(1000); // wait out stop bit
-
return byte;
}
diff --git a/zumo/pid.cpp b/zumo/pid.cpp
index 7c1cbda..17588e3 100644
--- a/zumo/pid.cpp
+++ b/zumo/pid.cpp
@@ -45,8 +45,7 @@ PID speed_pid = PID();
PID steer_pid = PID();
PID speed_mod_pid = PID();
void apply_pid(dui_state_t* target, dui_state_t* current) {
- current->speed = speed_pid.iter(target->speed);
+ current->Speed = speed_pid.iter(target->Speed);
current->steer = steer_pid.iter(target->steer);
current->speed_mod = speed_mod_pid.iter(target->speed_mod);
}
-
diff --git a/zumo/protocol.cpp b/zumo/protocol.cpp
index ab8397a..d176c24 100644
--- a/zumo/protocol.cpp
+++ b/zumo/protocol.cpp
@@ -1,5 +1,8 @@
-#include "protocol.h"
+#include <Zumo32U4.h>
+#include "protocol.h"
+#include <Arduino.h>
+#include <Zumo32U4LCD.h>
#define DUI_CMD_NULL 0x00
#define DUI_CMD_SIGN_START 0x01
#define DUI_CMD_SIGN_END 0x0f
@@ -7,15 +10,22 @@
#define DUI_CMD_SPEED_END 0x1f
#define DUI_CMD_STEER_START 0x20
#define DUI_CMD_STEER_END 0xff
+Zumo32U4OLED display;
void handle_cmd(unsigned char cmd, dui_state_t *state) {
+ Serial.println(cmd,HEX);
+
if (cmd == DUI_CMD_NULL) return;
else if (DUI_CMD_SIGN_START <= cmd && cmd <= DUI_CMD_SIGN_END) {
state->current_sign = (dui_e_sign) (cmd - DUI_CMD_SIGN_START);
+ display.clear();
+ display.print(state->current_sign+1);
+ display.gotoXY(0, 1);
} else if (DUI_CMD_SPEED_START <= cmd && cmd <= DUI_CMD_SPEED_END) {
- state->speed = (float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_END - DUI_CMD_SPEED_START);
+ Serial.print(" Hallo: " );
+ state->Speed = (float) (cmd - DUI_CMD_SPEED_START) / (float) (DUI_CMD_SPEED_END - DUI_CMD_SPEED_START);
+ Serial.println(state->Speed);
} else if (DUI_CMD_STEER_START <= cmd && cmd <= DUI_CMD_STEER_END) {
state->steer = (float) (cmd - DUI_CMD_STEER_START) / (float) (DUI_CMD_STEER_END - DUI_CMD_STEER_START) * (float) 2 - (float) 1;
}
}
-
diff --git a/zumo/protocol.h b/zumo/protocol.h
index a1f9951..5eb2343 100644
--- a/zumo/protocol.h
+++ b/zumo/protocol.h
@@ -21,11 +21,10 @@ typedef enum {
typedef struct {
float steer; /** @brief steer value (-1 is left, +1 is right) */
- float speed; /** @brief speed (0-15) */
+ float Speed; /** @brief speed (0-15) */
dui_e_sign current_sign; /** @brief last seen sign */
float speed_mod; /** @brief global speed multiplier */
} dui_state_t;
/** @brief read and apply cmd to state */
void handle_cmd(unsigned char cmd, dui_state_t *state);
-
diff --git a/zumo/zumo.ino b/zumo/zumo.ino
index 1a64381..a4f4d6b 100644
--- a/zumo/zumo.ino
+++ b/zumo/zumo.ino
@@ -2,19 +2,20 @@
#include <Arduino.h>
#include <Wire.h>
+
#include "control.h"
#include "protocol.h"
#include "pid.h"
dui_state_t g_dui_target_state = {
.steer = 0.0f,
- .speed = 0.0f,
+ .Speed = 0.0f,
.current_sign = DUI_SIGN_NONE,
.speed_mod = 1.f,
};
dui_state_t g_dui_current_state = {
.steer = 0.f,
- .speed = 0.f,
+ .Speed = 0.f,
.current_sign = DUI_SIGN_NONE,
.speed_mod = 1.f,
};
@@ -22,10 +23,13 @@ dui_state_t g_dui_current_state = {
void setup() {
pinMode(DUI_PINOUT_NICLA_TX, OUTPUT);
pinMode(DUI_PINOUT_NICLA_RX, INPUT_PULLUP);
- Serial.begin(115200);
+ Serial.begin(11500);
+
}
void loop() {
+
+
static unsigned char cmd_old = 0x00;
for (unsigned int i = 0; i < 1000; i++) {
digitalWrite(DUI_PINOUT_NICLA_TX, LOW);
@@ -40,4 +44,6 @@ void loop() {
g_dui_current_state.current_sign = g_dui_target_state.current_sign;
apply_state(&g_dui_current_state);
+
+
}