diff --git a/include/data_source.h b/include/data_source.h index 6d51e0c..56accba 100755 --- a/include/data_source.h +++ b/include/data_source.h @@ -4,6 +4,7 @@ #include "Arduino.h" #include "HardwareSerial.h" #include "vars.h" +#include "status_vector.h" class DataSource { diff --git a/include/status_vector.h b/include/status_vector.h new file mode 100644 index 0000000..cfaa29c --- /dev/null +++ b/include/status_vector.h @@ -0,0 +1,35 @@ +#pragma once + +#include + +#ifdef STATUS_VECTOR_CPP +#define sv_extr +#else +#define sv_extr extern +#endif + +#define dim 20 +#define CURRENT_DATA_READ (datas[(currentSVIndex-1) % dim]) +#define CURRENT_DATA_WRITE (datas[(currentSVIndex) % dim]) +#define CURRENT_INPUT_READ (inputs[(currentSVIndex-1) % dim]) +#define CURRENT_INPUT_WRITE (inputs[(currentSVIndex) % dim]) + +typedef struct input{ + int IMUAngle, USfr, USsx, USdx, USrr, BT; + byte ballByte, cameraByte, lineByte; + bool SW_DX, SW_SX; +}input; + +typedef struct data{ + int IMUAngle, ballAngle, ballDistance, cameraAngle, cameraDistance, + speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role; + byte xb, yb, xy, yy, lineSeen, lineActive; + bool mate, ATKgoal, DEFgoal; +}data; + +sv_extr input inputs[dim]; +sv_extr data datas[dim]; +sv_extr int currentSVIndex; + +void initStatusVector(); +void updateStatusVector(); \ No newline at end of file diff --git a/src/data_source_bno055.cpp b/src/data_source_bno055.cpp index af94fc0..049fef3 100755 --- a/src/data_source_bno055.cpp +++ b/src/data_source_bno055.cpp @@ -17,6 +17,10 @@ void DataSourceBNO055::readSensor(){ if(millis() - lastTime > DATA_CLOCK){ imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); this->value = (int) euler.x(); + + CURRENT_INPUT_WRITE.IMUAngle = this->value; + CURRENT_DATA_WRITE.IMUAngle = this->value; + lastTime = millis(); } } diff --git a/src/goalie.cpp b/src/goalie.cpp index 912087a..1787c64 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -45,8 +45,8 @@ void Goalie::goalie(int plusang) { } void Goalie::storcimentoPorta() { - if (camera->getValueAtk(false) >= 10 && camera->getValueAtk(false) <= 90) cstorc+=9; - else if (camera->getValueAtk(false) <= 350 && camera->getValueAtk(false) >= 270) cstorc-=9; + if (camera->getValueAtk(true ) >= 10 && camera->getValueAtk(true) <= 90) cstorc+=9; + else if (camera->getValueAtk(true) <= 350 && camera->getValueAtk(true) >= 270) cstorc-=9; // else cstorc *= 0.7; cstorc = constrain(cstorc, -45, 45); } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index a9f117f..0459db1 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -2,11 +2,13 @@ #include "sensors.h" #include "games.h" +#include "status_vector.h" void setup() { delay(1500); DEBUG.begin(9600); + initStatusVector(); initSensors(); initGames(); @@ -16,11 +18,11 @@ void setup() { void loop() { updateSensors(); - camera->test(); - // compass->test(); + goalie->play(role==1); keeper->play(role==0); - // Last thing to do: movement + // Last thing to do: movement and update status vector drive->drivePrepared(); + updateStatusVector(); } diff --git a/src/status_vector.cpp b/src/status_vector.cpp new file mode 100644 index 0000000..3c67ec1 --- /dev/null +++ b/src/status_vector.cpp @@ -0,0 +1,15 @@ +#define STATUS_VECTOR_CPP +#include "status_vector.h" + +void initStatusVector(){ + currentSVIndex = 0; + + for(int i=0; i>=dim; i++){ + inputs[i] = input(); + datas[i] = data(); + } +} + +void updateStatusVector(){ + currentSVIndex = (currentSVIndex+1) % dim; +} \ No newline at end of file diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index f8e8503..7984d9e 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -39,8 +39,8 @@ blue_led.on() -thresholds = [ (49, 99, -24, 17, 23, 64), # thresholds yellow goal - (-128,-128,-128,-128,-128,-128)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (30, 70, -12, 19, 10, 57), # thresholds yellow goal + (0, 31, -2, 39, -68, -25)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152) diff --git a/utility/OpenMV/main_test_conic.py b/utility/OpenMV/main_test_conic.py index a9dd948..5505073 100644 --- a/utility/OpenMV/main_test_conic.py +++ b/utility/OpenMV/main_test_conic.py @@ -29,8 +29,13 @@ blue_led.on() #thresholds = [ (54, 93, -10, 25, 55, 70), # thresholds yellow goal # (30, 45, 1, 40, -60, -19)] # thresholds blue goal # +<<<<<<< HEAD thresholds = [ (49, 84, -8, 31, 20, 80), # thresholds yellow goal (0, 51, -4, 44, -59, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +======= +thresholds = [ (30, 70, -12, 19, 41, 68) , # thresholds yellow goal + (0, 70, -2, 34, -59, -21)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +>>>>>>> 3c09f031f1833bf48d5dc14c166307217dee7fcf roi = (0, 6, 318, 152) @@ -52,9 +57,13 @@ sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) sensor.set_contrast(+2) sensor.set_saturation(+1) +<<<<<<< HEAD sensor.set_brightness(-3) +======= +sensor.set_brightness(-2) +>>>>>>> 3c09f031f1833bf48d5dc14c166307217dee7fcf sensor.set_quality(0) -sensor.set_auto_exposure(False, 8000) +sensor.set_auto_exposure(False, 6000) sensor.set_auto_gain(True) sensor.skip_frames(time = 300) @@ -79,7 +88,7 @@ while(True): tt_blue = [(0,999,0,2)] ## creo una lista di tuple per il blue, valore x = 999 : non trovata img = sensor.snapshot() - for blob in img.find_blobs(thresholds, pixels_threshold=100, area_threshold=350, merge = True): + for blob in img.find_blobs(thresholds, pixels_threshold=150, area_threshold=150, merge = True): img.draw_rectangle(blob.rect()) img.draw_cross(blob.cx(), blob.cy())