EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

code_midgen
EmaMaker 2020-01-26 20:13:31 +01:00
parent a9d9bc7928
commit dc251467bd
14 changed files with 345 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/data_source_camera.h
View File

@ -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;

140
src/data_source_camera.cpp
View File

@ -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("|");

15
src/main.cpp
View File

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

4
utility/OpenMV/conic_eff.py
View File

@ -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)

44
utility/Teensy-32u4/32u4_script/sketch_jan26b.ino Normal file
View File

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

5
utility/Teensy-32u4/Teensy32U4/.gitignore vendored Normal file
View File

@ -0,0 +1,5 @@
.pio
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch

67
utility/Teensy-32u4/Teensy32U4/.travis.yml Normal file
View File

@ -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

7
utility/Teensy-32u4/Teensy32U4/.vscode/extensions.json vendored Normal file
View File

@ -0,0 +1,7 @@
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
]
}

7
utility/Teensy-32u4/Teensy32U4/.vscode/settings.json vendored Normal file
View File

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

39
utility/Teensy-32u4/Teensy32U4/include/README Normal file
View File

@ -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

46
utility/Teensy-32u4/Teensy32U4/lib/README Normal file
View File

@ -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

15
utility/Teensy-32u4/Teensy32U4/platformio.ini Normal file
View File

@ -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

78
utility/Teensy-32u4/Teensy32U4/src/main.cpp Normal file
View File

@ -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;
}

11
utility/Teensy-32u4/Teensy32U4/test/README Normal file
View File

@ -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