Calculating angles with the camera, have to be fixed with the 90 degrees offset. Uploading a little teesny <-> atmega32u4 test script for bidirectional relaiable serial communication

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;

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(yAngleFix);
-    DEBUG.print(" --- ");
+    DEBUG.println(" --- ");
 
-    DEBUG.print(pDef);
+    DEBUG.print(bAngle);
     DEBUG.print(" | ");
-    DEBUG.println(fixCamIMU(pDef));  */
+    DEBUG.println(bAngleFix); 
-    //update();
     DEBUG.println("---------------");
     DEBUG.print(true_xb);
     DEBUG.print("|");

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
 } 

utility/OpenMV/conic_eff.py

@@ -39,8 +39,8 @@ blue_led.on()
 
 
 
-thresholds = [  (60, 75, -19, 9, 9, 60),    # thresholds yellow goal
+thresholds = [  (70, 100, -18, 21, 21, 63),    # thresholds yellow goal
-                (16, 35, -8, 24, -41, -13)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (31, 49, -10, 5, -39, -18)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 

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
+}

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

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

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"
+	]
+}

utility/Teensy-32u4/Teensy32U4/.vscode/settings.json

@@ -0,0 +1,7 @@
+{
+    "files.associations": {
+        "system_error": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp"
+    }
+}

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

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 <Foo.h>
+#include <Bar.h>
+
+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

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
+

utility/Teensy-32u4/Teensy32U4/src/main.cpp

@@ -0,0 +1,78 @@
+#include <Arduino.h>
+
+/*
+  * 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;
+}

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