From f2903c2c6bb0b3f826ff7e51398b9bf8836d3265 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Tue, 18 Feb 2020 09:37:36 +0100 Subject: [PATCH] Now using status vector everywhere :D (To be tested) --- .vscode/extensions.json | 12 +- include/data_source.h | 1 - include/data_source_bno055.h | 2 +- include/data_source_camera_conicmirror.h | 8 +- include/data_source_camera_vshapedmirror.h | 4 +- include/status_vector.h | 9 +- src/data_source_ball.cpp | 1 + src/data_source_bno055.cpp | 1 + src/data_source_camera_conicmirror.cpp | 29 ++-- src/data_source_camera_vshapedmirror.cpp | 58 +++++--- src/data_source_us.cpp | 55 +++++--- src/drivecontroller.cpp | 10 ++ src/game.cpp | 7 +- src/goalie.cpp | 5 +- src/keeper.cpp | 11 +- src/positionsys_zone.cpp | 29 ++-- utility/OpenMV/conic_eff.py | 2 +- utility/OpenMV/conic_eff.py.autosave | 149 --------------------- 18 files changed, 158 insertions(+), 235 deletions(-) delete mode 100644 utility/OpenMV/conic_eff.py.autosave diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 8281e64..0f0d740 100755 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,7 +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 + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] +} diff --git a/include/data_source.h b/include/data_source.h index 56accba..6d51e0c 100755 --- a/include/data_source.h +++ b/include/data_source.h @@ -4,7 +4,6 @@ #include "Arduino.h" #include "HardwareSerial.h" #include "vars.h" -#include "status_vector.h" class DataSource { diff --git a/include/data_source_bno055.h b/include/data_source_bno055.h index cb88188..d5de4ff 100755 --- a/include/data_source_bno055.h +++ b/include/data_source_bno055.h @@ -1,7 +1,7 @@ #pragma once -#include "data_source.h" #include +#include "data_source.h" #include #define DATA_CLOCK 10 diff --git a/include/data_source_camera_conicmirror.h b/include/data_source_camera_conicmirror.h index 72adcb1..aa32665 100755 --- a/include/data_source_camera_conicmirror.h +++ b/include/data_source_camera_conicmirror.h @@ -1,11 +1,13 @@ #pragma once + +#include "data_source.h" + #define startp 105 #define endp 115 //Coords are mapped from 0 up to this value #define MAP_MAX 100 #define HALF_MAP_MAX 50 //#define unkn 0b01101001 -#include "data_source.h" class DataSourceCameraConic : public DataSource{ @@ -13,8 +15,8 @@ class DataSourceCameraConic : public DataSource{ DataSourceCameraConic(HardwareSerial* ser, int baud); void test() override; void readSensor() override; - int getValueAtk(bool); - int getValueDef(bool); + // int getValueAtk(bool); + // int getValueDef(bool); int yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist; diff --git a/include/data_source_camera_vshapedmirror.h b/include/data_source_camera_vshapedmirror.h index efb3ba7..53ee3b2 100644 --- a/include/data_source_camera_vshapedmirror.h +++ b/include/data_source_camera_vshapedmirror.h @@ -9,8 +9,8 @@ class DataSourceCameraVShaped : public DataSource{ void test() override; int fixCamIMU(int); void readSensor() override; - int getValueAtk(bool); - int getValueDef(bool); + // int getValueAtk(bool); + // int getValueDef(bool); int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB; int cameraReady; diff --git a/include/status_vector.h b/include/status_vector.h index a9e18d1..c940f54 100644 --- a/include/status_vector.h +++ b/include/status_vector.h @@ -1,5 +1,7 @@ #pragma once #include +#include "game.h" +#include "systems.h" /** * STATUS VECTOR: @@ -33,8 +35,11 @@ typedef struct input{ }input; typedef struct data{ - int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, - speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role; + int IMUAngle, ballAngle, ballDistance, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, angleAtk, angleAtkFix, angleDef, angleDefFix, + speed, tilt, dir, USfr, USsx, USdx, USrr, lineOutDir, matePos, role, axisBlock[4]; + Game* game; + LineSystem* lineSystem; + PositionSystem* posSystem; byte xb, yb, xy, yy, lineSeen, lineActive; bool mate, ATKgoal, DEFgoal, ballSeen; }data; diff --git a/src/data_source_ball.cpp b/src/data_source_ball.cpp index 71fea22..fcb5267 100755 --- a/src/data_source_ball.cpp +++ b/src/data_source_ball.cpp @@ -1,5 +1,6 @@ #include "data_source_ball.h" #include "vars.h" +#include "status_vector.h" DataSourceBall :: DataSourceBall(HardwareSerial* ser_, int baud) : DataSource(ser_, baud) { ballSeen = false; diff --git a/src/data_source_bno055.cpp b/src/data_source_bno055.cpp index e94408b..6c6c59e 100755 --- a/src/data_source_bno055.cpp +++ b/src/data_source_bno055.cpp @@ -1,4 +1,5 @@ #include "data_source_bno055.h" +#include "status_vector.h" //bool loaded = false; diff --git a/src/data_source_camera_conicmirror.cpp b/src/data_source_camera_conicmirror.cpp index 7780f6a..65fc83a 100755 --- a/src/data_source_camera_conicmirror.cpp +++ b/src/data_source_camera_conicmirror.cpp @@ -1,5 +1,5 @@ #include "data_source_camera_conicmirror.h" -#include "sensors.h" +#include "status_vector.h" DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){ true_xb = 0; @@ -78,18 +78,29 @@ void DataSourceCameraConic :: readSensor(){ CURRENT_DATA_WRITE.bAngleFix = bAngleFix; CURRENT_DATA_WRITE.yDist = yDist; CURRENT_DATA_WRITE.bDist = bDist; + if(goalOrientation == HIGH){ + CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle; + CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix; + CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.bAngle; + CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix; + }else{ + CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle; + CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix; + CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle; + CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix; + } } } -int DataSourceCameraConic::getValueAtk(bool fixed){ - if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix; - else return goalOrientation == HIGH ? yAngle : bAngle; -} -int DataSourceCameraConic::getValueDef(bool fixed){ - if(fixed) return goalOrientation == LOW ? yAngleFix : bAngleFix; - else return goalOrientation == LOW ? yAngle : bAngle; -} +// int DataSourceCameraConic::getValueAtk(bool fixed){ +// if(fixed) return goalOrientation == HIGH ? yAngleFix : bAngleFix; +// else return goalOrientation == HIGH ? yAngle : bAngle; +// } +// 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 diff --git a/src/data_source_camera_vshapedmirror.cpp b/src/data_source_camera_vshapedmirror.cpp index d658827..63de4ca 100644 --- a/src/data_source_camera_vshapedmirror.cpp +++ b/src/data_source_camera_vshapedmirror.cpp @@ -1,5 +1,7 @@ #include "data_source_camera_vshapedmirror.h" #include "sensors.h" +#include "status_vector.h" + DataSourceCameraVShaped::DataSourceCameraVShaped(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){} @@ -64,7 +66,6 @@ void DataSourceCameraVShaped :: readSensor(){ } void DataSourceCameraVShaped :: postProcess(){ - if (valY != -74) oldGoalY = valY; if (valB != -74) @@ -90,33 +91,46 @@ void DataSourceCameraVShaped :: postProcess(){ datavalid = 0; cameraReady = 1; //attivo flag di ricezione pacchetto } -} -int DataSourceCameraVShaped::getValueAtk(bool fixed){ //attacco gialla if(goalOrientation == HIGH){ - if(fixed) return fixCamIMU(valY); - return valY; - } - //attacco blu - if(goalOrientation == LOW){ - if(fixed) return fixCamIMU(valB); - return valB; + CURRENT_DATA_WRITE.angleAtkFix = fixCamIMU(valY); + CURRENT_DATA_WRITE.angleAtk = valY; + CURRENT_DATA_WRITE.angleDef = fixCamIMU(valB); + CURRENT_DATA_WRITE.angleDefFix = valB; + }else{ + CURRENT_DATA_WRITE.angleAtkFix = fixCamIMU(valB); + CURRENT_DATA_WRITE.angleAtkFix = valB; + CURRENT_DATA_WRITE.angleDef = fixCamIMU(valY); + CURRENT_DATA_WRITE.angleDefFix = valY; } } -int DataSourceCameraVShaped::getValueDef(bool fixed){ - //difendo gialla - if(goalOrientation == HIGH){ - if(fixed) return fixCamIMU(valY); - return valY; - } - //difendo blu - if(goalOrientation == LOW){ - if(fixed) return fixCamIMU(valB); - return valB; - } -} +// int DataSourceCameraVShaped::getValueAtk(bool fixed){ +// //attacco gialla +// if(goalOrientation == HIGH){ +// if(fixed) return fixCamIMU(valY); +// return valY; +// } +// //attacco blu +// if(goalOrientation == LOW){ +// if(fixed) return fixCamIMU(valB); +// return valB; +// } +// } + +// int DataSourceCameraVShaped::getValueDef(bool fixed){ +// //difendo gialla +// if(goalOrientation == HIGH){ +// if(fixed) return fixCamIMU(valY); +// return valY; +// } +// //difendo blu +// if(goalOrientation == LOW){ +// if(fixed) return fixCamIMU(valB); +// return valB; +// } +// } void DataSourceCameraVShaped::test(){ goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu diff --git a/src/data_source_us.cpp b/src/data_source_us.cpp index 63a325c..b2431b3 100755 --- a/src/data_source_us.cpp +++ b/src/data_source_us.cpp @@ -1,5 +1,7 @@ #include "data_source_us.h" #include "vars.h" +#include "status_vector.h" + DataSourceUS::DataSourceUS(TwoWire* i2c_, int addr) : DataSource(i2c_, addr){ us_flag = false; @@ -76,22 +78,41 @@ void DataSourceUS::usTrigger() { } void DataSourceUS::usReceive() { - // transmit to device #112s - i2c->beginTransmission(i2CAddr); - // sets register pointer to echo 1 register(0x02) - i2c->write(byte(0x02)); - i2c->endTransmission(); + // transmit to device #112s + i2c->beginTransmission(i2CAddr); + // sets register pointer to echo 1 register(0x02) + i2c->write(byte(0x02)); + i2c->endTransmission(); - // step 4: request reading from sensor - // request 2 bytes from slave device #112 - i2c->requestFrom(i2CAddr, 2); + // step 4: request reading from sensor + // request 2 bytes from slave device #112 + i2c->requestFrom(i2CAddr, 2); - // step 5: receive reading from sensor - // receive high byte (overwrites previous reading) - reading = i2c->read(); - // shift high byte to be high 8 bits - reading = reading << 8; - // receive low byte as lower 8 bit - reading |= i2c->read(); - value = reading; - } \ No newline at end of file + // step 5: receive reading from sensor + // receive high byte (overwrites previous reading) + reading = i2c->read(); + // shift high byte to be high 8 bits + reading = reading << 8; + // receive low byte as lower 8 bit + reading |= i2c->read(); + value = reading; + + switch(i2CAddr){ + case 112: + CURRENT_INPUT_WRITE.USfr = value; + CURRENT_DATA_WRITE.USfr = value; + break; + case 113: + CURRENT_INPUT_WRITE.USdx = value; + CURRENT_DATA_WRITE.USdx = value; + break; + case 114: + CURRENT_INPUT_WRITE.USrr = value; + CURRENT_DATA_WRITE.USrr = value; + break; + case 115: + CURRENT_INPUT_WRITE.USsx = value; + CURRENT_DATA_WRITE.USsx = value; + break; + } +} \ No newline at end of file diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index 534b6b9..fae1728 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -1,5 +1,6 @@ #include "drivecontroller.h" #include "sensors.h" +#include "status_vector.h" DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_){ m1 = m1_; @@ -60,6 +61,7 @@ float DriveController::torad(float f){ } void DriveController::drive(int dir, int speed, int tilt){ + vx = ((speed * cosins[dir])); vy = ((-speed * sins[dir])); @@ -102,4 +104,12 @@ void DriveController::drive(int dir, int speed, int tilt){ m2->drive((int) speed2); m3->drive((int) speed3); m4->drive((int) speed4); + + CURRENT_DATA_WRITE.dir = dir; + CURRENT_DATA_WRITE.speed = speed; + CURRENT_DATA_WRITE.tilt = tilt; + CURRENT_DATA_WRITE.axisBlock[0] = vxp; + CURRENT_DATA_WRITE.axisBlock[1] = vxn; + CURRENT_DATA_WRITE.axisBlock[2] = vyp; + CURRENT_DATA_WRITE.axisBlock[3] = vyn; } \ No newline at end of file diff --git a/src/game.cpp b/src/game.cpp index 87619f4..744ae02 100644 --- a/src/game.cpp +++ b/src/game.cpp @@ -1,4 +1,5 @@ #include "game.h" +#include "status_vector.h" Game::Game() {} Game::Game(LineSystem* ls_, PositionSystem* ps_) { @@ -11,6 +12,10 @@ void Game::play(bool condition){ if(condition) { realPlay(); ls->update(); - } + CURRENT_DATA_WRITE.posSystem = (this->ps); + CURRENT_DATA_WRITE.lineSystem = (this->ls); + CURRENT_DATA_WRITE.game = this; + + } } \ No newline at end of file diff --git a/src/goalie.cpp b/src/goalie.cpp index 444ed0b..5c95a0d 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -1,6 +1,7 @@ #include "goalie.h" #include "sensors.h" #include "vars.h" +#include "status_vector.h" #include "positionsys_zone.h" Goalie::Goalie() : Game() { @@ -46,8 +47,8 @@ void Goalie::goalie(int plusang) { } void Goalie::storcimentoPorta() { - if (camera->getValueAtk(true ) >= 10 && camera->getValueAtk(true) <= 90) cstorc+=9; - else if (camera->getValueAtk(true) <= 350 && camera->getValueAtk(true) >= 270) cstorc-=9; + if (CURRENT_DATA_READ.angleAtkFix >= 10 && CURRENT_DATA_READ.angleAtkFix <= 90) cstorc+=9; + else if (CURRENT_DATA_READ.angleAtkFix <= 350 && CURRENT_DATA_READ.angleAtkFix >= 270) cstorc-=9; // else cstorc *= 0.7; cstorc = constrain(cstorc, -45, 45); } \ No newline at end of file diff --git a/src/keeper.cpp b/src/keeper.cpp index 1b6e8c3..b534fc8 100755 --- a/src/keeper.cpp +++ b/src/keeper.cpp @@ -3,6 +3,7 @@ #include "games.h" #include "linesys_2019.h" #include +#include "status_vector.h" Keeper::Keeper() : Game() { init(); @@ -44,8 +45,8 @@ void Keeper::keeper() { angle = (KEEPER_ANGLE_DX + ball->angle) * M_PI / 180; angleX = abs(cos(angle)); - if(ball->angle >= 0 && ball->angle <= KEEPER_ANGLE_DX && camera->getValueDef(true) < 30) drive->prepareDrive(KEEPER_ANGLE_DX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT); - else if(ball->angle >= KEEPER_ANGLE_SX && ball->angle <= 360 && camera->getValueDef(true) > -30) drive->prepareDrive(KEEPER_ANGLE_SX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT); + if(ball->angle >= 0 && ball->angle <= KEEPER_ANGLE_DX && CURRENT_DATA_READ.angleDefFix < 30) drive->prepareDrive(KEEPER_ANGLE_DX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT); + else if(ball->angle >= KEEPER_ANGLE_SX && ball->angle <= 360 && CURRENT_DATA_READ.angleDefFix > -30) drive->prepareDrive(KEEPER_ANGLE_SX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT); else if(ball->angle < KEEPER_ANGLE_SX && ball->angle > KEEPER_ANGLE_DX){ int ball_degrees2 = ball->angle > 180? ball->angle-360:ball->angle; int dir = ball_degrees2 > 0 ? ball->angle + KEEPER_BALL_BACK_ANGLE : ball->angle - KEEPER_BALL_BACK_ANGLE; @@ -57,11 +58,11 @@ void Keeper::keeper() { } void Keeper::centerGoalPostCamera(bool checkBack){ - if (camera->getValueDef(true) > CENTERGOALPOST_CAM_MAX) { + if (CURRENT_DATA_READ.angleDefFix > CENTERGOALPOST_CAM_MAX) { drive->prepareDrive(KEEPER_ANGLE_SX, CENTERGOALPOST_VEL1); - } else if (camera->getValueDef(true) < CENTERGOALPOST_CAM_MIN) { + } else if (CURRENT_DATA_READ.angleDefFix < CENTERGOALPOST_CAM_MIN) { drive->prepareDrive(KEEPER_ANGLE_DX, CENTERGOALPOST_VEL1); - }else if(camera->getValueDef(true) > CENTERGOALPOST_CAM_MIN && camera->getValueDef(true) < CENTERGOALPOST_CAM_MAX){ + }else if(CURRENT_DATA_READ.angleDefFix > CENTERGOALPOST_CAM_MIN && CURRENT_DATA_READ.angleDefFix < CENTERGOALPOST_CAM_MAX){ if(!ball->ballSeen) drive->prepareDrive(0, 0, 0); if(checkBack){ if(usCtrl->getValue(2) > CENTERGOALPOST_US_MAX){ diff --git a/src/positionsys_zone.cpp b/src/positionsys_zone.cpp index c763411..b12ba93 100644 --- a/src/positionsys_zone.cpp +++ b/src/positionsys_zone.cpp @@ -1,6 +1,7 @@ #include "positionsys_zone.h" #include "vars.h" #include "sensors.h" +#include "status_vector.h" PositionSysZone::PositionSysZone(){ for(int i = 0; i < 3; i++){ @@ -174,17 +175,17 @@ void PositionSysZone::phyZoneUS(){ else DxF = DxF_Def; - Lx_mis = usCtrl->getValue(1) + usCtrl->getValue(3) + robot; // larghezza totale stimata - Ly_mis = usCtrl->getValue(0) + usCtrl->getValue(2) + robot; // lunghezza totale stimata + Lx_mis = CURRENT_DATA_READ.USdx + CURRENT_DATA_READ.USsx + robot; // larghezza totale stimata + Ly_mis = CURRENT_DATA_READ.USfr + CURRENT_DATA_READ.USrr + robot; // lunghezza totale stimata // controllo orizzontale - if ((Lx_mis < Lx_max) && (Lx_mis > Lx_min) && (usCtrl->getValue(1) > 25) && (usCtrl->getValue(3) > 25)) { + if ((Lx_mis < Lx_max) && (Lx_mis > Lx_min) && (CURRENT_DATA_READ.USdx > 25) && (CURRENT_DATA_READ.USsx > 25)) { // se la misura orizzontale é accettabile good_field_x = true; status_x = CENTER; - if (usCtrl->getValue(1) < DxF) // robot é vicino al bordo dEASTro + if (CURRENT_DATA_READ.USdx < DxF) // robot é vicino al bordo dEASTro status_x = EAST; - if (usCtrl->getValue(3) < DxF) // robot é vicino al bordo sinistro + if (CURRENT_DATA_READ.USsx < DxF) // robot é vicino al bordo sinistro status_x = WEST; if (status_x == CENTER) { @@ -200,7 +201,7 @@ void PositionSysZone::phyZoneUS(){ } } else { // la misura non é pulita per la presenza di un ostacolo - if ((usCtrl->getValue(1) >= (DxF + 10)) || (usCtrl->getValue(3) >= (DxF + 10))) { + if ((CURRENT_DATA_READ.USdx >= (DxF + 10)) || (CURRENT_DATA_READ.USsx >= (DxF + 10))) { // se ho abbastanza spazio a dEASTra o a sinistra // devo stare per forza al cento status_x = CENTER; @@ -233,19 +234,19 @@ void PositionSysZone::phyZoneUS(){ // se la misura verticale é accettabile good_field_y = true; status_y = CENTER; - if (usCtrl->getValue(0) < Dy) { + if (CURRENT_DATA_READ.USfr < Dy) { status_y = NORTH; // robot é vicino alla porta avversaria if (Dy == DyP) goal_zone = true; // davanti alla porta in zona goal } - if (usCtrl->getValue(2) < Dy) + if (CURRENT_DATA_READ.USrr < Dy) status_y = SOUTH; // robot é vicino alla propria porta } else { // la misura non é pulita per la presenza di un ostacolo status_y = 255; // non so la coordinata y - if (usCtrl->getValue(0) >= (Dy + 0)) + if (CURRENT_DATA_READ.USfr >= (Dy + 0)) status_y = CENTER; // ma se ho abbastanza spazio dietro o avanti - if (usCtrl->getValue(2) >= (Dy + 0)) + if (CURRENT_DATA_READ.USrr >= (Dy + 0)) status_y = CENTER; // e'probabile che stia al CENTER } @@ -262,9 +263,9 @@ void PositionSysZone::phyZoneUS(){ void PositionSysZone::phyZoneCam(){ //IMU-fixed attack angle - camA = camera->getValueAtk(true); + camA = CURRENT_DATA_READ.angleAtkFix; //IMU-fixed defence angle - camD = camera->getValueDef(true); + camD = CURRENT_DATA_READ.angleDefFix; //Negative angle means that the robot is positioned to the right of the goalpost //Positive angle means that the robot is positioned to the left of the goalpost @@ -416,7 +417,7 @@ void PositionSysZone::goCenter() { // drive->prepareDrive(270, vel); // } else { // stop_menamoli = false; -// if (usCtrl->getValue(2) >= 25) +// if (CURRENT_DATA_READ.USrr >= 25) // drive->prepareDrive(180, vel); // else // drive->prepareDrive(0, 0); @@ -425,7 +426,7 @@ void PositionSysZone::goCenter() { void PositionSysZone::AAANGOLO() { - if((usCtrl->getValue(2) <= 45) && ((usCtrl->getValue(1) <= 50) || (usCtrl->getValue(3) <= 50))) drive->prepareDrive(0, 350, 0); + if((CURRENT_DATA_READ.USrr <= 45) && ((CURRENT_DATA_READ.USdx <= 50) || (CURRENT_DATA_READ.USsx <= 50))) drive->prepareDrive(0, 350, 0); } int PositionSysZone::diff(int a, int b){ diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index 05cb677..9a3c96d 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -51,7 +51,7 @@ sensor.set_framesize(sensor.QVGA) sensor.skip_frames(time = 2000) sensor.set_auto_gain(False) # must be turned off for color tracking sensor.set_auto_whitebal(False) # must be turned off for color tracking -sensor.set_auto_exposure(False, 10000) +sensor.set_auto_exposure(False, 10000) vbc #sensor.set_backlight(1) #sensor.set_brightness(+2) #sensor.set_windowing(roi) diff --git a/utility/OpenMV/conic_eff.py.autosave b/utility/OpenMV/conic_eff.py.autosave deleted file mode 100644 index 05cb677..0000000 --- a/utility/OpenMV/conic_eff.py.autosave +++ /dev/null @@ -1,149 +0,0 @@ -# color tracking with conic mirror - By: EmaMaker - wed 15 jan 2020 -# Based on: -# color tracking - By: paolix - ven mag 18 2018 - -# Automatic RGB565 Color Tracking Example -# - -import sensor, image, time, pyb, math - -from pyb import UART -uart = UART(3,19200, timeout_char = 1000) - -START_BYTE = chr(105) #'i' -END_BYTE = chr(115) #'s' -BYTE_UNKNOWN = chr(116) #'t' - -y_found = False -b_found = False - -#From Arduino Documentation at: https://www.arduino.cc/reference/en/language/functions/math/map/ -def val_map(x, in_min, in_max, out_min, out_max): - x = int(x) - in_min = int(in_min) - in_max = int(in_max) - out_min = int(out_min) - out_max = int(out_max) - return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min) - - -# LED Setup ################################################################## -red_led = pyb.LED(1) -green_led = pyb.LED(2) -blue_led = pyb.LED(3) - -red_led.off() -green_led.off() -blue_led.on() -############################################################################## - - - -thresholds = [ (0, 99, -16, 19, 13, 85), # thresholds yellow goal - (26, 52, -8, 19, -49, -18)] # thresholds blue goal (6, 31, -15, 4, -35, 0) - -roi = (0, 6, 318, 152) - -# Camera Setup ############################################################### -'''sensor.reset() -sensor.set_pixformat(sensor.RGB565) -sensor.set_framesize(sensor.QVGA) -sensor.skip_frames(time = 2000) -sensor.set_auto_gain(False) # must be turned off for color tracking -sensor.set_auto_whitebal(False) # must be turned off for color tracking -sensor.set_auto_exposure(False, 10000) -#sensor.set_backlight(1) -#sensor.set_brightness(+2) -#sensor.set_windowing(roi) -clock = time.clock()''' - -sensor.reset() -sensor.set_pixformat(sensor.RGB565) -sensor.set_framesize(sensor.QQVGA) -sensor.set_contrast(+3) -sensor.set_saturation(0) -sensor.set_brightness(-2) -sensor.set_quality(0) -sensor.set_auto_exposure(False, 10000) -sensor.set_auto_gain(True) -sensor.skip_frames(time = 300) - -clock = time.clock() -############################################################################## - - -while(True): - clock.tick() - - blue_led.off() - - y_found = False - b_found = False - - tt_yellow = [(0,999,0,1)] ## creo una lista di tuple per il giallo, valore x = 999 : non trovata - 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=130, merge = True): - img.draw_rectangle(blob.rect()) - img.draw_cross(blob.cx(), blob.cy()) - - if (blob.code() == 1): - tt_yellow = tt_yellow + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ] - y_found = True - if (blob.code() == 2): - tt_blue = tt_blue + [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ] - b_found = True - - tt_yellow.sort(key=lambda tup: tup[0]) ## ordino le liste - tt_blue.sort(key=lambda tup: tup[0]) ## ordino le liste - - ny = len(tt_yellow) - nb = len(tt_blue) - - y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1] - b_area, b1_cx, b1_cy, b_code = tt_blue[nb-1] - - y_cx = int(img.width() / 2 - y1_cx) - y_cy = int(img.height() / 2 - y1_cy) - b_cx = int(img.width() / 2 - b1_cx) - b_cy = int(img.height() / 2 - b1_cy) - - #Normalize data between 0 and 100 - if y_found == True: - y_cx = val_map(y_cx, -img.width() / 2, img.width() / 2, 100, 0) - y_cy = val_map(y_cy, -img.height() / 2, img.height() / 2, 0, 100) - #Prepare for send as a list of characters - s_ycx = chr(y_cx) - s_ycy = chr(y_cy) - else: - y_cx = BYTE_UNKNOWN - y_cy = BYTE_UNKNOWN - #Prepare for send as a list of characters - s_ycx = y_cx - s_ycy = y_cy - - if b_found == True: - b_cx = val_map(b_cx, -img.width() / 2, img.width() / 2, 100, 0) - b_cy = val_map(b_cy, -img.height() / 2, img.height() / 2, 0, 100) - - #Prepare for send as a list of characters - s_bcx = chr(b_cx) - s_bcy = chr(b_cy) - else: - b_cx = BYTE_UNKNOWN - b_cy = BYTE_UNKNOWN - #Prepare for send as a list of characters - s_bcx = b_cx - s_bcy = b_cy - - print(str(y_cx) + " | " + str(y_cy) + " --- " + str(b_cx) + " | " + str(b_cy)) - - - uart.write(START_BYTE) - uart.write(s_bcx) - uart.write(s_bcy) - uart.write(s_ycx) - uart.write(s_ycy) - uart.write(END_BYTE) -