diff --git a/include/data_source_camera.h b/include/data_source_camera.h index 60cdae0..d8aaff6 100755 --- a/include/data_source_camera.h +++ b/include/data_source_camera.h @@ -15,6 +15,8 @@ class DataSourceCamera : public DataSource{ int getValueAtk(bool); int getValueDef(bool); + int yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist; + int count = 0, unkn_counter; byte xb, yb, xy, yy, true_xb, true_xy, true_yb, true_yy; bool data_received = false, start = false, end = false; diff --git a/src/data_source_camera.cpp b/src/data_source_camera.cpp index 53a22f9..c29b976 100755 --- a/src/data_source_camera.cpp +++ b/src/data_source_camera.cpp @@ -13,16 +13,27 @@ void DataSourceCamera :: readSensor(){ } else if(value==endp){ data_received=false; -<<<<<<< HEAD if(count=4 && start==true) { -======= - if(count==4 && start==true) { ->>>>>>> d534c5b4a8c2064cb43c42f9269b7f0256894ef8 data_received=true; true_xb = xb; true_yb = yb; true_xy = xy; true_yy = yy; + + //Remap to [-50, +49] to correctly compute angles and distances + true_xb -= 50; + true_yb -= 50; + true_xy -= 50; + true_yy -= 50; + + //Now calculate angles and distance + yAngle = atan2(true_yy, true_xy) * 180 / 3.14; + bAngle = atan2(true_yb, true_xb) * 180 / 3.14; + yAngleFix = yAngle - compass->getValue() ; + bAngleFix = bAngle - compass->getValue() ; + + yDist = sqrt( true_yy*true_yy + true_xy*true_xy ); + bDist = sqrt( true_yb*true_yb + true_xb*true_xb ); } end=true; start=false; @@ -39,117 +50,7 @@ void DataSourceCamera :: readSensor(){ } -/* - for(int i=0;i<6;i++) reading[i]=(char)ser->read(); - if(reading[0]==startp) start=true; - else if(reading[0]==endp && start==true) continue; - if(reading[1]==startp && (start==true || count==0)) continue; //ser->flush(); //ignoring data and continuing - else if (reading[1] == unkn) unkn_counter++;//xb = oldxb; - else reading[1]=xb; - count++; - if(reading[2]==startp && start==true) break; - else reading[2]=yb; - count++; - if(reading[3]==startp && start==true) break; - else if (reading[3] == unkn) unkn_counter++;//xy = oldxy; //tried to do for now - else reading[3]=xy; - count++; - if(reading[4]==startp && start==true) break; - else reading[4]=yy; - count++; - if(reading[5]==endp && count==4){ - end=true; - data_received=true; - start=false; - } -*/ - /* portx = 999; - while(ser->available() > 0) { - value = ser->read(); - // if the incoming character is a 'Y', set the start packet flag - if (value == 'Y') { - startpY = 1; - } - // if the incoming character is a 'Y', set the start packet flag - if (value == 'B') { - startpB = 1; - } - // if the incoming character is a '.', set the end packet flag - if (value == 'y') { - endpY = 1; - } - // if the incoming character is a '.', set the end packet flag - if (value == 'b') { - endpB = 1; - } - - if ((startpY == 1) && (endpY == 0)) { - if (isDigit(value)) { - // convert the incoming byte to a char and add it to the string: - valStringY += value; - }else if(value == '-'){ - negateY = true; - } - } - - if ((startpB == 1) && (endpB == 0)) { - if (isDigit(value)) { - // convert the incoming byte to a char and add it to the string: - valStringB += value; - }else if(value == '-'){ - negateB = true; - } - } - - if ((startpY == 1) && (endpY == 1)) { - valY = valStringY.toInt(); // valid data - if(negateY) valY *= -1; - valStringY = ""; - startpY = 0; - endpY = 0; - negateY = false; - datavalid ++; - } - if ((startpB == 1) && (endpB == 1)) { - valB = valStringB.toInt(); // valid data - if(negateB) valB *= -1; - valStringB = ""; - startpB = 0; - endpB = 0; - negateB = false; - datavalid ++; - } - } -} -*/ - void DataSourceCamera :: postProcess(){ - - /*if (valY != -74) - oldGoalY = valY; - if (valB != -74) - oldGoalB = valB; - - if (valY == -74) - valY = oldGoalY; - if (valB == -74) - valB = oldGoalB; - - // entro qui solo se ho ricevuto i pacchetti completi sia del blu che del giallo - if (datavalid > 1 ) { - if(goalOrientation == 1){ - //yellow goalpost - pAtk = valY; - pDef = valB * -1; - }else{ - //blue goalpost - pAtk = valB; - pDef = valY * -1; - } - - datavalid = 0; - cameraReady = 1; //attivo flag di ricezione pacchetto - }*/ } int DataSourceCamera::getValueAtk(bool fixed){ @@ -181,15 +82,14 @@ int DataSourceCamera::getValueDef(bool fixed){ void DataSourceCamera::test(){ goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu update(); -/* DEBUG.print(pAtk); + DEBUG.print(yAngle); DEBUG.print(" | "); - DEBUG.print(fixCamIMU(pAtk)); - DEBUG.print(" --- "); + DEBUG.print(yAngleFix); + DEBUG.println(" --- "); - DEBUG.print(pDef); + DEBUG.print(bAngle); DEBUG.print(" | "); - DEBUG.println(fixCamIMU(pDef)); */ - //update(); + DEBUG.println(bAngleFix); DEBUG.println("---------------"); DEBUG.print(true_xb); DEBUG.print("|"); diff --git a/src/main.cpp b/src/main.cpp index 86ba22b..a654c77 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -16,24 +16,13 @@ void setup() { void loop() { updateSensors(); -<<<<<<< HEAD //camera->test(); goalie->play(role==1); keeper->play(role==0); - ball->test(); - + camera->test(); + // Last thing to do: movement drive->drivePrepared(); -======= - - // goalie->play(role==1); - // keeper->play(role==0); - - // Last thing to do: movement - // drive->drivePrepared(); - //Serial.print("ao"); - camera->test(); ->>>>>>> d534c5b4a8c2064cb43c42f9269b7f0256894ef8 } diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index a06c5f6..002bc52 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -39,8 +39,8 @@ blue_led.on() -thresholds = [ (60, 75, -19, 9, 9, 60), # thresholds yellow goal - (16, 35, -8, 24, -41, -13)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (70, 100, -18, 21, 21, 63), # thresholds yellow goal + (31, 49, -10, 5, -39, -18)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (0, 6, 318, 152) diff --git a/utility/Teensy-32u4/32u4_script/sketch_jan26b.ino b/utility/Teensy-32u4/32u4_script/sketch_jan26b.ino new file mode 100644 index 0000000..5b0a884 --- /dev/null +++ b/utility/Teensy-32u4/32u4_script/sketch_jan26b.ino @@ -0,0 +1,44 @@ +#define BTN0 A3 +#define BTN1 A4 +#define BTN2 A5 +#define LED_Y 11 +#define LED_R 17 + +void setup (){ + Serial1.begin(19200); + + pinMode(LED_Y,OUTPUT); + pinMode(LED_R,OUTPUT); + pinMode(BTN0,INPUT); + pinMode(BTN1,INPUT); + pinMode(BTN2,INPUT); +} + +byte b, oldB = 0; + +void loop () { + //We have three config. buttons, making a packet out of a single byte with a start header, so teensy can use it as it wants + //The header is signed by the two most important bits put high, so 128+64 in OR with other bits shifted by the needed ammount + //This approach only admits 5 configuration buttons, it should be enough + b = 0b11000000; + b |= digitalRead(BTN0); + b |= digitalRead(BTN1) << 1; + b |= digitalRead(BTN2) << 2; + if(oldB != b) Serial1.write(b); + oldB = b; + + while(Serial1.available() > 0){ + int a = Serial1.read(); + if((a & 0b11000000) == 0b11000000){ + digitalWrite(LED_Y, !(a & 0b00000001)); + digitalWrite(LED_R, !(a & 0b00000010)); + } + } + int angle = (int) (atan2(3,2) * 180 / 3.14); + angle = angle / 2 + angle % 2; + int dist = 256; + Serial1.write((char) 105); //Start packet + Serial1.write((char) angle); //Start packet + Serial1.write((char) dist); //Start packet + Serial1.write((char) 115); //Start packet +} diff --git a/utility/Teensy-32u4/Teensy32U4/.gitignore b/utility/Teensy-32u4/Teensy32U4/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/utility/Teensy-32u4/Teensy32U4/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/utility/Teensy-32u4/Teensy32U4/.travis.yml b/utility/Teensy-32u4/Teensy32U4/.travis.yml new file mode 100644 index 0000000..7c486f1 --- /dev/null +++ b/utility/Teensy-32u4/Teensy32U4/.travis.yml @@ -0,0 +1,67 @@ +# Continuous Integration (CI) is the practice, in software +# engineering, of merging all developer working copies with a shared mainline +# several times a day < https://docs.platformio.org/page/ci/index.html > +# +# Documentation: +# +# * Travis CI Embedded Builds with PlatformIO +# < https://docs.travis-ci.com/user/integration/platformio/ > +# +# * PlatformIO integration with Travis CI +# < https://docs.platformio.org/page/ci/travis.html > +# +# * User Guide for `platformio ci` command +# < https://docs.platformio.org/page/userguide/cmd_ci.html > +# +# +# Please choose one of the following templates (proposed below) and uncomment +# it (remove "# " before each line) or use own configuration according to the +# Travis CI documentation (see above). +# + + +# +# Template #1: General project. Test it using existing `platformio.ini`. +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# install: +# - pip install -U platformio +# - platformio update +# +# script: +# - platformio run + + +# +# Template #2: The project is intended to be used as a library with examples. +# + +# language: python +# python: +# - "2.7" +# +# sudo: false +# cache: +# directories: +# - "~/.platformio" +# +# env: +# - PLATFORMIO_CI_SRC=path/to/test/file.c +# - PLATFORMIO_CI_SRC=examples/file.ino +# - PLATFORMIO_CI_SRC=path/to/test/directory +# +# install: +# - pip install -U platformio +# - platformio update +# +# script: +# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N diff --git a/utility/Teensy-32u4/Teensy32U4/.vscode/extensions.json b/utility/Teensy-32u4/Teensy32U4/.vscode/extensions.json new file mode 100644 index 0000000..272828b --- /dev/null +++ b/utility/Teensy-32u4/Teensy32U4/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} \ No newline at end of file diff --git a/utility/Teensy-32u4/Teensy32U4/.vscode/settings.json b/utility/Teensy-32u4/Teensy32U4/.vscode/settings.json new file mode 100644 index 0000000..ef0a485 --- /dev/null +++ b/utility/Teensy-32u4/Teensy32U4/.vscode/settings.json @@ -0,0 +1,7 @@ +{ + "files.associations": { + "system_error": "cpp", + "type_traits": "cpp", + "utility": "cpp" + } +} \ No newline at end of file diff --git a/utility/Teensy-32u4/Teensy32U4/include/README b/utility/Teensy-32u4/Teensy32U4/include/README new file mode 100644 index 0000000..194dcd4 --- /dev/null +++ b/utility/Teensy-32u4/Teensy32U4/include/README @@ -0,0 +1,39 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the usual convention is to give header files names that end with `.h'. +It is most portable to use only letters, digits, dashes, and underscores in +header file names, and at most one dot. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/utility/Teensy-32u4/Teensy32U4/lib/README b/utility/Teensy-32u4/Teensy32U4/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/utility/Teensy-32u4/Teensy32U4/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/utility/Teensy-32u4/Teensy32U4/platformio.ini b/utility/Teensy-32u4/Teensy32U4/platformio.ini new file mode 100644 index 0000000..1751470 --- /dev/null +++ b/utility/Teensy-32u4/Teensy32U4/platformio.ini @@ -0,0 +1,15 @@ +;PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:teensy35] +platform = teensy +board = teensy35 +framework = arduino + diff --git a/utility/Teensy-32u4/Teensy32U4/src/main.cpp b/utility/Teensy-32u4/Teensy32U4/src/main.cpp new file mode 100644 index 0000000..e12d50a --- /dev/null +++ b/utility/Teensy-32u4/Teensy32U4/src/main.cpp @@ -0,0 +1,78 @@ +#include + +/* + * Author: EmaMaker on 26 Jan 2020 + * Experimenting with Teensy 3.5 <-> Atmega32u4 Bidirectional Serial communication +*/ + +#define LED1 32 +#define LED2 31 +#define LED3 30 + +void setup() { + delay(2000); + Serial5.begin(19200); + Serial.begin(9600); + + pinMode(LED1, OUTPUT); + pinMode(LED2, OUTPUT); + pinMode(LED3, OUTPUT); + + delay(2000); + + Serial.println("Teensy 3.5 working!"); +} + +int value; + +int startp = 105; +int endp = 115; +bool start, end, data_received; +int led1 = 0, led2 = 0, led3 = 0, count = 0; +int angle, dist, angle_temp, dist_temp; +//As when transimitting from 32u4 to teensy, we use the same syntax to send a byte representing max 6 states to be displayed on LEDs on 32u4 +byte b, oldB = 0; +void loop() { + while(Serial5.available() > 0){ + value = (int)Serial5.read(); + if((value & 0b11000000) == 0b11000000){ + + led1 = value & 0b0000001; + led2 = value & 0b0000010; + led3 = value & 0b0000100; + digitalWrite(LED1, led1); + digitalWrite(LED2, led2); + digitalWrite(LED3, led3); + }else{ + if(value==startp){ + start=true; + count=0; + } + else if(value==endp){ + data_received=false; + if(count==2 && start==true) { + data_received=true; + angle = angle_temp; + dist = dist_temp; + Serial.print(angle); + Serial.print(" | "); + Serial.println(dist); + } + end=true; + start=false; + }else{ + if(start==true){ + if (count==0) angle_temp=value; + else if (count==1) dist_temp=value; + count++; + } + } + } + } + b = 0b11000000; + b |= led1; + b |= led2; + b |= led3; + if(oldB != b) Serial5.write(b); + oldB = b; +} \ No newline at end of file diff --git a/utility/Teensy-32u4/Teensy32U4/test/README b/utility/Teensy-32u4/Teensy32U4/test/README new file mode 100644 index 0000000..df5066e --- /dev/null +++ b/utility/Teensy-32u4/Teensy32U4/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html