From e943ed3992044c601c3eb036a07ff9f68db52c8b Mon Sep 17 00:00:00 2001 From: heavydemon21 Date: Wed, 7 Jun 2023 13:54:17 +0200 Subject: zumo update --- nicla/road.py | 67 +++++++++++++++++++++++++-------------------------- nicla/signs_detect.py | 2 +- zumo/control.cpp | 10 +++----- zumo/pid.cpp | 3 +-- zumo/protocol.cpp | 16 +++++++++--- zumo/protocol.h | 3 +-- zumo/zumo.ino | 12 ++++++--- 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 +#include "protocol.h" +#include +#include #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 #include + #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); + + } -- cgit v1.2.3