From df0b37554ce1854472454b9fa27970eca5843364 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Wed, 29 Jan 2020 18:56:49 +0100 Subject: [PATCH] reintroduces semi axis locking. Some bug with the lines make the robot stop when it gets on one. Tilt with vshaped mirror reintroduced --- ...era.h => data_source_camera_conicmirror.h} | 4 +- include/data_source_camera_vshapedmirror.h | 26 ++++ include/drivecontroller.h | 9 +- include/goalie.h | 2 +- include/linesys_2019.h | 1 - include/sensors.h | 5 +- ...cpp => data_source_camera_conicmirror.cpp} | 17 ++- src/data_source_camera_vshapedmirror.cpp | 142 ++++++++++++++++++ src/drivecontroller.cpp | 17 +++ src/goalie.cpp | 60 +------- src/linesys_2019.cpp | 4 +- src/main.cpp | 4 +- src/sensors.cpp | 2 +- 13 files changed, 212 insertions(+), 81 deletions(-) rename include/{data_source_camera.h => data_source_camera_conicmirror.h} (85%) create mode 100644 include/data_source_camera_vshapedmirror.h rename src/{data_source_camera.cpp => data_source_camera_conicmirror.cpp} (81%) create mode 100644 src/data_source_camera_vshapedmirror.cpp diff --git a/include/data_source_camera.h b/include/data_source_camera_conicmirror.h similarity index 85% rename from include/data_source_camera.h rename to include/data_source_camera_conicmirror.h index e46795e..7bead6b 100755 --- a/include/data_source_camera.h +++ b/include/data_source_camera_conicmirror.h @@ -7,10 +7,10 @@ //#define unkn 0b01101001 #include "data_source.h" -class DataSourceCamera : public DataSource{ +class DataSourceCameraConic : public DataSource{ public: - DataSourceCamera(HardwareSerial* ser, int baud); + DataSourceCameraConic(HardwareSerial* ser, int baud); void test() override; void readSensor() override; int getValueAtk(bool); diff --git a/include/data_source_camera_vshapedmirror.h b/include/data_source_camera_vshapedmirror.h new file mode 100644 index 0000000..efb3ba7 --- /dev/null +++ b/include/data_source_camera_vshapedmirror.h @@ -0,0 +1,26 @@ +#pragma once +#include "data_source.h" + +class DataSourceCameraVShaped : public DataSource{ + + public: + DataSourceCameraVShaped(HardwareSerial* ser, int baud); + void postProcess() override; + void test() override; + int fixCamIMU(int); + void readSensor() override; + int getValueAtk(bool); + int getValueDef(bool); + + int goalOrientation, pAtk, pDef, imuOff, portx, valX, valY, valB, oldGoalX, oldGoalY, oldGoalB; + int cameraReady; + char value; + int startpY = 0; + int startpB = 0; + int endpY = 0; + int endpB = 0; + int datavalid = 0; + String valStringY = ""; + String valStringB = ""; + bool negateB, negateY; +}; \ No newline at end of file diff --git a/include/drivecontroller.h b/include/drivecontroller.h index 9ec8aa2..512fce3 100755 --- a/include/drivecontroller.h +++ b/include/drivecontroller.h @@ -4,10 +4,11 @@ #include "motor.h" //PID Constants - -#define KP 0.9 +#define KP 2.1 #define KI 0 -#define KD 0 +#define KD 0.05 + +#define UNLOCK_THRESH 800 class DriveController{ @@ -23,7 +24,7 @@ class DriveController{ int vxp, vyp, vxn, vyn; bool canUnlock; - elapsedMillis unlockTime; + unsigned long unlockTime; private: Motor* m1; diff --git a/include/goalie.h b/include/goalie.h index 60d495d..2c03032 100755 --- a/include/goalie.h +++ b/include/goalie.h @@ -2,7 +2,7 @@ #include "game.h" #include "sensors.h" -#include "data_source_camera.h" +#include "data_source_camera_vshapedmirror.h" #define GOALIE_ATKSPD_LAT 255 #define GOALIE_ATKSPD_BAK 350 diff --git a/include/linesys_2019.h b/include/linesys_2019.h index 6275203..60e2f3d 100755 --- a/include/linesys_2019.h +++ b/include/linesys_2019.h @@ -36,5 +36,4 @@ class LineSys2019 : public LineSystem{ elapsedMillis exitTimer; int outDir, outVel; byte linesens; - unsigned long unlockTime; }; \ No newline at end of file diff --git a/include/sensors.h b/include/sensors.h index 915c3b7..1782296 100755 --- a/include/sensors.h +++ b/include/sensors.h @@ -10,7 +10,8 @@ #include #include "data_source_bno055.h" #include "data_source_ball.h" -#include "data_source_camera.h" +#include "data_source_camera_conicmirror.h" +#include "data_source_camera_vshapedmirror.h" #include "data_source_us.h" #include "motor.h" #include "ds_ctrl.h" @@ -30,7 +31,7 @@ s_extr LineSys2019* linesCtrl; s_extr DataSourceBNO055* compass; s_extr DataSourceBall* ball; -s_extr DataSourceCamera* camera; +s_extr DataSourceCameraVShaped* camera; s_extr DriveController* drive; s_extr DataSourceBT* bt; diff --git a/src/data_source_camera.cpp b/src/data_source_camera_conicmirror.cpp similarity index 81% rename from src/data_source_camera.cpp rename to src/data_source_camera_conicmirror.cpp index 565c084..cc600ef 100755 --- a/src/data_source_camera.cpp +++ b/src/data_source_camera_conicmirror.cpp @@ -1,9 +1,9 @@ -#include "data_source_camera.h" +#include "data_source_camera_conicmirror.h" #include "sensors.h" -DataSourceCamera::DataSourceCamera(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){} +DataSourceCameraConic::DataSourceCameraConic(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){} -void DataSourceCamera :: readSensor(){ +void DataSourceCameraConic :: readSensor(){ while(ser->available() > 0){ value = (int)ser->read(); //Serial.println(value); @@ -31,8 +31,9 @@ void DataSourceCamera :: readSensor(){ yAngle = (yAngle + 360) % 360; bAngle = (bAngle + 360) % 360; - yAngleFix = yAngle - compass->getValue()*0.9 ; - bAngleFix = bAngle - compass->getValue()*0.9 ; + //Fixes with IMU + yAngleFix = yAngle - compass->getValue()*0.85 ; + bAngleFix = bAngle - compass->getValue()*0.85 ; 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) ); @@ -52,14 +53,14 @@ void DataSourceCamera :: readSensor(){ } -int DataSourceCamera::getValueAtk(bool b){ +int DataSourceCameraConic::getValueAtk(bool b){ return 0; } -int DataSourceCamera::getValueDef(bool b){ +int DataSourceCameraConic::getValueDef(bool b){ return 0; } -void DataSourceCamera::test(){ +void DataSourceCameraConic::test(){ goalOrientation = digitalRead(SWITCH_SX); //se HIGH attacco gialla, difendo blu update(); DEBUG.print(bAngle); diff --git a/src/data_source_camera_vshapedmirror.cpp b/src/data_source_camera_vshapedmirror.cpp new file mode 100644 index 0000000..d658827 --- /dev/null +++ b/src/data_source_camera_vshapedmirror.cpp @@ -0,0 +1,142 @@ +#include "data_source_camera_vshapedmirror.h" +#include "sensors.h" + +DataSourceCameraVShaped::DataSourceCameraVShaped(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){} + +void DataSourceCameraVShaped :: readSensor(){ + 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 DataSourceCameraVShaped :: 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 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 + + + DEBUG.print(pAtk); + DEBUG.print(" | "); + DEBUG.print(fixCamIMU(pAtk)); + DEBUG.print(" --- "); + + DEBUG.print(pDef); + DEBUG.print(" | "); + DEBUG.println(fixCamIMU(pDef)); + delay(100); +} + +int DataSourceCameraVShaped::fixCamIMU(int d){ + if(compass->getValue() > 0 && compass->getValue() < 180) imuOff = compass->getValue(); + else if (compass->getValue() <= 360 && compass->getValue() >= 180) imuOff = compass->getValue() - 360; + imuOff = constrain(imuOff*0.8, -30, 30); + + return d + imuOff; +} \ No newline at end of file diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp index af2653d..42f4d66 100755 --- a/src/drivecontroller.cpp +++ b/src/drivecontroller.cpp @@ -32,6 +32,13 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) errorePre = 0; pidfactor = 0; integral = 0; + canUnlock = true; + unlockTime = 0; + + vxp = 0; + vxn = 0; + vyp = 0; + vyn = 0; } void DriveController::prepareDrive(int dir, int speed, int tilt){ @@ -52,6 +59,16 @@ void DriveController::drive(int dir, int speed, int tilt){ vx = ((speed * cosins[dir])); vy = ((-speed * sins[dir])); + if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) { + vxn = 0; + vxp = 0; + vyp = 0; + vyn = 0; + } + + if((vy > 0 && vxn == 1) || (vy < 0 && vxp == 1)) vy = 0; + if((vx > 0 && vyp == 1) || (vx < 0 && vyn == 1)) vx = 0; + speed1 = ((vx * sins[m1->angle] ) + (vy * cosins[m1->angle] )); speed2 = ((vx * sins[m2->angle]) + (vy * cosins[m2->angle])); speed3 = -(speed1); diff --git a/src/goalie.cpp b/src/goalie.cpp index 4fadbd0..4d01848 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -34,51 +34,11 @@ void Goalie::goalie(int plusang) { if(ball->dir < 0) ball->dir = ball->dir + 360; else ball->dir = ball->dir; ball->b = ball->dir; - drive->prepareDrive(ball->dir, 300, 0); - } - /* drive->speed = 300; - drive->dir = drive->dir; */ - /* if(ball->angle >= 350 || ball->angle <= 10) { - if(ball->distance > 190) atk_direction = 0; - else atk_direction = ball->angle; - atk_speed = GOALIE_ATKSPD_FRT; - } - if(ball->angle >= 90 && ball->angle <= 270) { - this->ballBack(); - atk_speed = GOALIE_ATKSPD_BAK; + storcimentoPorta(); + if(ball->angle > 340 || ball->angle < 20) drive->prepareDrive(ball->dir, 350, cstorc); + else drive->prepareDrive(ball->dir, 350, 0); } - if(ball->angle > 10 && ball->angle < 30) { - atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG1; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle >= 30 && ball->angle < 45) { - atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG2; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle >= 45 && ball->angle < 90) { - atk_direction = ball->angle + GOALIE_ATKDIR_PLUSANG3; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle > 270 && ball->angle <= 315) { - atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG3_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle > 315 && ball->angle <= 330) { - atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG2_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - if(ball->angle > 330 && ball->angle < 350) { - atk_direction = ball->angle - GOALIE_ATKDIR_PLUSANG1_COR; - atk_speed = GOALIE_ATKSPD_LAT; - } - - this->storcimentoPorta(); - if((ball->angle >= 330 || ball->angle <= 30) && ball->distance > 190) { //storcimento - atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito - drive->prepareDrive(atk_direction, atk_speed, cstorc); - } - else drive->prepareDrive(atk_direction, atk_speed); */ } void Goalie::storcimentoPorta() { @@ -86,18 +46,4 @@ void Goalie::storcimentoPorta() { else if (camera->getValueAtk(true) < -3) cstorc-=9; else cstorc *= 0.7; cstorc = constrain(cstorc, -45, 45); -} - -void Goalie::ballBack() { - -/* if(ball->distance > 130) ball->plusang = GOALIE_ATKDIR_PLUSANGBAK; - else ball->plusang = 0; - - if(ball->angle > 180) ball->degrees2 = ball->angle - 360; - else ball->degrees2 = ball->angle; - if(ball->degrees2 > 0) ball->dir = ball->angle + ball->plusang; //45 con 8 ruote - else ball->dir = ball->angle - ball->plusang; //45 con 8 ruote - if(ball->dir < 0) ball->dir = ball->dir + 360; - else ball->dir = ball->dir; - atk_direction = ball->dir; */ } \ No newline at end of file diff --git a/src/linesys_2019.cpp b/src/linesys_2019.cpp index c9a8641..ef416b1 100755 --- a/src/linesys_2019.cpp +++ b/src/linesys_2019.cpp @@ -78,7 +78,7 @@ void LineSys2019::outOfBounds(){ if (exitTimer <= EXTIME){ //fase di rientro if(linesens == 15) linesens = linesensOldY | linesensOldX; //ZOZZATA MAXIMA - unlockTime = millis(); + drive->unlockTime = millis(); if(linesens == 1) outDir = 180; else if(linesens == 2) outDir = 270; @@ -153,7 +153,7 @@ void LineSys2019::outOfBounds(){ if(linesensOldY == 4) drive->vyn = 1; else if(linesensOldY == 1) drive->vyp = 1; } - + drive->canUnlock = true; linesens = 0; linesensOldY = 0; linesensOldX = 0; diff --git a/src/main.cpp b/src/main.cpp index 7281864..c9be453 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -16,12 +16,10 @@ void setup() { void loop() { updateSensors(); - //camera->test(); + // camera->test(); goalie->play(role==1); keeper->play(role==0); -camera->test(); - // Last thing to do: movement drive->drivePrepared(); } diff --git a/src/sensors.cpp b/src/sensors.cpp index 78cb8ce..cdf95f5 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 DataSourceCamera(&Serial2, 19200); + camera = new DataSourceCameraVShaped(&Serial2, 19200); usCtrl = new DataSourceCtrl(dUs); bt = new DataSourceBT(&Serial3, 115200); }