From 6acc62c1a3b240f7d8ad9440099270f72ac1a834 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Fri, 31 Jan 2020 13:35:40 +0100 Subject: [PATCH] Fixed camera angle reading from both openmv and teensy. Tilt from goalie kinda working now --- include/data_source_bno055.h | 3 ++ include/data_source_camera_conicmirror.h | 2 +- include/drivecontroller.h | 3 +- include/sensors.h | 2 +- include/vars.h | 2 +- src/data_source_bno055.cpp | 8 +++- src/data_source_bt.cpp | 2 +- src/data_source_camera_conicmirror.cpp | 60 ++++++++++++++++-------- src/drivecontroller.cpp | 5 ++ src/goalie.cpp | 13 +++-- src/linesys_2019.cpp | 2 +- src/main.cpp | 5 +- src/sensors.cpp | 2 +- utility/OpenMV/conic_eff.py | 16 +++---- 14 files changed, 81 insertions(+), 44 deletions(-) diff --git a/include/data_source_bno055.h b/include/data_source_bno055.h index 36a01c0..cb88188 100755 --- a/include/data_source_bno055.h +++ b/include/data_source_bno055.h @@ -4,6 +4,8 @@ #include #include +#define DATA_CLOCK 10 + class DataSourceBNO055 : public DataSource{ public: @@ -12,5 +14,6 @@ class DataSourceBNO055 : public DataSource{ public: Adafruit_BNO055 bno; + unsigned long lastTime; }; \ No newline at end of file diff --git a/include/data_source_camera_conicmirror.h b/include/data_source_camera_conicmirror.h index 7bead6b..72adcb1 100755 --- a/include/data_source_camera_conicmirror.h +++ b/include/data_source_camera_conicmirror.h @@ -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; diff --git a/include/drivecontroller.h b/include/drivecontroller.h index 512fce3..0336a1a 100755 --- a/include/drivecontroller.h +++ b/include/drivecontroller.h @@ -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); diff --git a/include/sensors.h b/include/sensors.h index 1782296..d71275e 100755 --- a/include/sensors.h +++ b/include/sensors.h @@ -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; diff --git a/include/vars.h b/include/vars.h index e8fec13..560cb30 100755 --- a/include/vars.h +++ b/include/vars.h @@ -1,5 +1,5 @@ #pragma once -#define DEBUG Serial +#define DEBUG Serial3 #define LED_R 20 #define LED_Y 17 diff --git a/src/data_source_bno055.cpp b/src/data_source_bno055.cpp index aff1970..af94fc0 100755 --- a/src/data_source_bno055.cpp +++ b/src/data_source_bno055.cpp @@ -10,9 +10,13 @@ DataSourceBNO055::DataSourceBNO055(){ bno.setExtCrystalUse(true); //loaded = true; value = 0; + lastTime = 0; } void DataSourceBNO055::readSensor(){ - imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); - this->value = (int) euler.x(); + if(millis() - lastTime > DATA_CLOCK){ + imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); + this->value = (int) euler.x(); + lastTime = millis(); + } } diff --git a/src/data_source_bt.cpp b/src/data_source_bt.cpp index 565f260..ae170aa 100644 --- a/src/data_source_bt.cpp +++ b/src/data_source_bt.cpp @@ -1,7 +1,7 @@ #include "data_source_bt.h" DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){ - connect(); + // connect(); } void DataSourceBT :: connect(){ diff --git a/src/data_source_camera_conicmirror.cpp b/src/data_source_camera_conicmirror.cpp index cc600ef..0013a0e 100755 --- a/src/data_source_camera_conicmirror.cpp +++ b/src/data_source_camera_conicmirror.cpp @@ -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); diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index 42f4d66..3a25e2c 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -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); } diff --git a/src/goalie.cpp b/src/goalie.cpp index 4d01848..740ea29 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -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); } \ No newline at end of file diff --git a/src/linesys_2019.cpp b/src/linesys_2019.cpp index ef416b1..b4497b4 100755 --- a/src/linesys_2019.cpp +++ b/src/linesys_2019.cpp @@ -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 diff --git a/src/main.cpp b/src/main.cpp index c9be453..fbb18fa 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -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); diff --git a/src/sensors.cpp b/src/sensors.cpp index cdf95f5..1e72cdb 100755 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -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); } diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 677bb8a..1f9f231 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -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)