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

Fixed camera angle reading from both openmv and teensy. Tilt from goalie kinda working now

code_midgen
EmaMaker 2020-01-31 13:35:40 +01:00
parent df0b37554c
commit 6acc62c1a3
14 changed files with 81 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/data_source_bno055.h
View File

@ -4,6 +4,8 @@
#include <Adafruit_BNO055.h>
#include <Arduino.h>
#define DATA_CLOCK 10
class DataSourceBNO055 : public DataSource{
public:
@ -12,5 +14,6 @@ class DataSourceBNO055 : public DataSource{
public:
Adafruit_BNO055 bno;
unsigned long lastTime;
};

2
include/data_source_camera_conicmirror.h
View File

@ -19,7 +19,7 @@ class DataSourceCameraConic : public DataSource{
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, calc_xb, calc_yb, calc_xy, calc_yy;
int xb, yb, xy, yy, true_xb, true_xy, true_yb, true_yy, calc_xb, calc_yb, calc_xy, calc_yy;
bool data_received = false, start = false, end = false;
int goalOrientation, pAtk, pDef;

3
include/drivecontroller.h
View File

@ -17,7 +17,8 @@ class DriveController{
DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_);
void drive(int dir=0, int speed=0, int tilt=0);
void prepareDrive(int dir=0, int speed=0, int tilt=0);
void prepareDrive(int dir, int speed, int tilt);
void prepareDrive(int dir, int speed);
void drivePrepared();
float updatePid();
float torad(float f);

2
include/sensors.h
View File

@ -31,7 +31,7 @@ s_extr LineSys2019* linesCtrl;
s_extr DataSourceBNO055* compass;
s_extr DataSourceBall* ball;
s_extr DataSourceCameraVShaped* camera;
s_extr DataSourceCameraConic* camera;
s_extr DriveController* drive;
s_extr DataSourceBT* bt;

2
include/vars.h
View File

@ -1,5 +1,5 @@
#pragma once
#define DEBUG Serial
#define DEBUG Serial3
#define LED_R 20
#define LED_Y 17

4
src/data_source_bno055.cpp
View File

@ -10,9 +10,13 @@ DataSourceBNO055::DataSourceBNO055(){
bno.setExtCrystalUse(true);
//loaded = true;
value = 0;
lastTime = 0;
}
void DataSourceBNO055::readSensor(){
if(millis() - lastTime > DATA_CLOCK){
imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
this->value = (int) euler.x();
lastTime = millis();
}
}

2
src/data_source_bt.cpp
View File

@ -1,7 +1,7 @@
#include "data_source_bt.h"
DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){
connect();
// connect();
}
void DataSourceBT :: connect(){

60
src/data_source_camera_conicmirror.cpp
View File

@ -1,7 +1,25 @@
#include "data_source_camera_conicmirror.h"
#include "sensors.h"
DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){}
DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){
true_xb = 0;
true_yb = 0;
true_xy = 0;
true_yy = 0;
xb = 0;
yb = 0;
xy = 0;
yy = 0;
start = false;
data_received = false;
end = false;
yAngle = 0;
yAngleFix = 0;
yDist = 0;
bAngle = 0;
bAngleFix = 0;
bDist = 0;
}
void DataSourceCameraConic :: readSensor(){
while(ser->available() > 0){
@ -16,27 +34,24 @@ void DataSourceCameraConic :: readSensor(){
if(count=4 && start==true) {
data_received=true;
true_xb = xb;
true_yb = yb;
true_xy = xy;
true_yy = yy;
true_xb = xb-50;
true_yb = 50-yb;
true_xy = xy-50;
true_yy = 50-yy;
//Remap from [0,100] to [-50, +49] to correctly compute angles and distances and calculate them
yAngle = atan2(50-true_yy, 50-true_xy) * 180 / 3.14;
bAngle = atan2(50-true_yb, 50-true_xb) * 180 / 3.14;
//Subtract 90 to bring the angles back to euler angles (0 in front)
yAngle = 90 - yAngle;
bAngle = 90 - bAngle;
yAngle = -90-(atan2(true_yy, true_xy) * 180 /3.14);
bAngle = -90-(atan2(true_yb, true_xb)* 180 /3.14);
//Now cast angles to [0, 359] domain angle flip them
yAngle = (yAngle + 360) % 360;
bAngle = (bAngle + 360) % 360;
yAngle = (yAngle+360)%360;
bAngle = (bAngle+360)%360;
//Fixes with IMU
yAngleFix = yAngle - compass->getValue()*0.85 ;
bAngleFix = bAngle - compass->getValue()*0.85 ;
yAngleFix = ((int)(yAngle - compass->getValue()*0.35) + 360) % 360 ;
bAngleFix = ((int)(bAngle - compass->getValue()*0.35) + 360) % 360 ;
yDist = sqrt( (50-true_yy)*(50-true_yy) + (50-true_xy)*(50-true_xy) );
bDist = sqrt( (50-true_yb)*(50-true_yb) + (50-true_xb)*(50-true_xb) );
yDist = sqrt( (true_yy-50)*(true_yy-50) + (50-true_xy)*(50-true_xy) );
bDist = sqrt( (true_yb-50)*(true_yb-50) + (50-true_xb)*(50-true_xb) );
}
end=true;
start=false;
@ -53,16 +68,19 @@ void DataSourceCameraConic :: readSensor(){
}
int DataSourceCameraConic::getValueAtk(bool b){
return 0;
int DataSourceCameraConic::getValueAtk(bool fixed){
if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix;
else return goalOrientation == HIGH ? yAngle : bAngle;
}
int DataSourceCameraConic::getValueDef(bool b){
return 0;
int DataSourceCameraConic::getValueDef(bool fixed){
if(fixed) return goalOrientation == LOW ? yAngleFix : bAngleFix;
else return goalOrientation == LOW ? yAngle : bAngle;
}
void DataSourceCameraConic::test(){
goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu
update();
DEBUG.print("Blue: ");
DEBUG.print(bAngle);
DEBUG.print(" | ");
DEBUG.print(bAngleFix);
@ -70,12 +88,14 @@ void DataSourceCameraConic::test(){
DEBUG.println(bDist);
DEBUG.println(" --- ");
DEBUG.print("Yellow: ");
DEBUG.print(yAngle);
DEBUG.print(" | ");
DEBUG.print(yAngleFix);
DEBUG.print(" | ");
DEBUG.println(yDist);
DEBUG.println("---------------");
DEBUG.print("Data: ");
DEBUG.print(true_xb);
DEBUG.print("|");
DEBUG.print(true_yb);

5
src/drivecontroller.cpp
View File

@ -47,6 +47,11 @@ void DriveController::prepareDrive(int dir, int speed, int tilt){
pTilt = tilt;
}
void DriveController::prepareDrive(int dir, int speed){
pDir = dir;
pSpeed = speed;
}
void DriveController::drivePrepared(){
drive(pDir, pSpeed, pTilt);
}

13
src/goalie.cpp
View File

@ -36,14 +36,17 @@ void Goalie::goalie(int plusang) {
ball->b = ball->dir;
storcimentoPorta();
if(ball->angle > 340 || ball->angle < 20) drive->prepareDrive(ball->dir, 350, cstorc);
else drive->prepareDrive(ball->dir, 350, 0);
if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 150, cstorc);
else {
drive->prepareDrive(ball->dir, 150, 0);
cstorc = 0;
}
}
}
void Goalie::storcimentoPorta() {
if (camera->getValueAtk(true) >= 3) cstorc+=9;
else if (camera->getValueAtk(true) < -3) cstorc-=9;
else cstorc *= 0.7;
if (camera->getValueAtk(false) >= 10 && camera->getValueAtk(false) <= 90) cstorc+=9;
else if (camera->getValueAtk(false) <= 350 && camera->getValueAtk(false) >= 270) cstorc-=9;
// else cstorc *= 0.7;
cstorc = constrain(cstorc, -45, 45);
}

2
src/linesys_2019.cpp
View File

@ -101,7 +101,7 @@ void LineSys2019::outOfBounds(){
else if(linesensOldY == 1) outDir = 180;
}
drive->prepareDrive(outDir, LINES_EXIT_SPD, 0);
drive->prepareDrive(outDir, LINES_EXIT_SPD);
tookLine = true;
}else{
//fine rientro

5
src/main.cpp
View File

@ -4,19 +4,20 @@
#include "games.h"
void setup() {
delay(1000);
delay(1500);
DEBUG.begin(9600);
initSensors();
initGames();
delay(2000);
delay(1500);
}
void loop() {
updateSensors();
// camera->test();
// compass->test();
goalie->play(role==1);
keeper->play(role==0);

2
src/sensors.cpp
View File

@ -15,7 +15,7 @@ void initSensors(){
drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
compass = new DataSourceBNO055();
ball = new DataSourceBall(&Serial4, 57600);
camera = new DataSourceCameraVShaped(&Serial2, 19200);
camera = new DataSourceCameraConic(&Serial2, 19200);
usCtrl = new DataSourceCtrl(dUs);
bt = new DataSourceBT(&Serial3, 115200);
}

16
utility/OpenMV/conic_eff.py
View File

@ -39,8 +39,8 @@ blue_led.on()
thresholds = [ (44, 100, -4, 48, 11, 74), # thresholds yellow goal
(16, 60, -4, 48, -73, -22)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
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)
roi = (0, 6, 318, 152)
@ -61,10 +61,10 @@ sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_contrast(+3)
sensor.set_saturation(+1)
sensor.set_brightness(-1)
sensor.set_saturation(0)
sensor.set_brightness(-3)
sensor.set_quality(0)
sensor.set_auto_exposure(False, 12000)
sensor.set_auto_exposure(False, 6000)
sensor.set_auto_gain(True)
sensor.skip_frames(time = 300)
@ -84,7 +84,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=75, area_threshold=115, merge = True):
for blob in img.find_blobs(thresholds, pixels_threshold=75, area_threshold=130, merge = True):
img.draw_rectangle(blob.rect())
img.draw_cross(blob.cx(), blob.cy())
@ -141,9 +141,9 @@ while(True):
uart.write(START_BYTE)
uart.write(s_ycx)
uart.write(s_ycy)
uart.write(s_bcx)
uart.write(s_bcy)
uart.write(s_ycx)
uart.write(s_ycy)
uart.write(END_BYTE)