diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index db63b3a..f93cf2f 100644 --- a/include/behaviour_control/status_vector.h +++ b/include/behaviour_control/status_vector.h @@ -29,14 +29,14 @@ #define CURRENT_INPUT_WRITE ( inputs[((currentSVIndex))] ) typedef struct input{ - int IMUAngle, USfr, USsx, USdx, USrr, BT; + int IMUAngle, USfr, USsx, USdx, USrr, BT, ballPresenceVal; byte cameraByte, ballByte, lineByte, xb, yb, xy, yy; bool SW_DX, SW_SX; String ballString; }input; typedef struct data{ - int IMUAngle, ballAngle, ballAngleFix, ballDistance, + int IMUAngle, ballAngle, ballAngleFix, ballDistance, ballPresenceVal, yAngle, bAngle, yAngleFix, bAngleFix, yDist, bDist, angleAtk, angleAtkFix, angleDef, angleDefFix, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix, @@ -53,7 +53,7 @@ typedef struct data{ bool mate, ATKgoal, DEFgoal, atkSeen, defSeen, bSeen, ySeen, - ballSeen; + ballSeen, ballPresent; }data; sv_extr input inputs[dim]; diff --git a/include/sensors/data_source_ball_presence.h b/include/sensors/data_source_ball_presence.h new file mode 100644 index 0000000..2190d17 --- /dev/null +++ b/include/sensors/data_source_ball_presence.h @@ -0,0 +1,18 @@ +#pragma once + +#define BALL_PRESENCE_TRESH 500 + +#include +#include "behaviour_control/data_source.h" + +class DataSourceBallPresence : public DataSource{ + + public: + DataSourceBallPresence(int, bool); + void postProcess() override; + void test() override; + bool isInMouth(); + + public: + bool present; +}; \ No newline at end of file diff --git a/include/sensors/sensors.h b/include/sensors/sensors.h index 0575c17..390ffa7 100644 --- a/include/sensors/sensors.h +++ b/include/sensors/sensors.h @@ -17,6 +17,7 @@ #include "systems/lines/linesys_camera.h" #include "systems/position/positionsys_zone.h" #include "sensors/data_source_ball.h" +#include "sensors/data_source_ball_presence.h" #include "sensors/data_source_bt.h" #include "sensors/data_source_bno055.h" #include "sensors/data_source_camera_conicmirror.h" @@ -35,6 +36,7 @@ s_extr DataSourceBall* ball; s_extr DataSourceCameraConic* camera; s_extr DriveController* drive; s_extr DataSourceBT* bt; +s_extr DataSourceBallPresence* ballPresence; s_extr int role; s_extr int robot_indentifier; diff --git a/src/main.cpp b/src/main.cpp index 565c08e..47a56bc 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -9,8 +9,6 @@ #include "test_menu.h" #include "motors_movement/roller.h" -void updateRoller(); - TestMenu* testmenu; #define BALL_IN A13 @@ -53,12 +51,12 @@ void loop() { drive->resetDrive(); // striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike; - keeper_condition = role == LOW; + // keeper_condition = role == LOW; - striker->play(1); + precision_shooter->play(1); testmenu->testMenu(); - + // Last thing to do: movement and update status vector drive->drivePrepared(); updateStatusVector(); diff --git a/src/sensors/data_source_ball_presence.cpp b/src/sensors/data_source_ball_presence.cpp new file mode 100644 index 0000000..1ad3e09 --- /dev/null +++ b/src/sensors/data_source_ball_presence.cpp @@ -0,0 +1,29 @@ +#include "sensors/data_source_ball_presence.h" +#include "behaviour_control/status_vector.h" +#include "vars.h" + +DataSourceBallPresence::DataSourceBallPresence(int pin_, bool analog) : DataSource(pin_, analog){ + present = false; + value = 0; +} + +void DataSourceBallPresence::postProcess(){ + present = value <= BALL_PRESENCE_TRESH; + + CURRENT_INPUT_WRITE.ballPresenceVal = value; + CURRENT_DATA_WRITE.ballPresenceVal = value; + CURRENT_DATA_WRITE.ballPresent = present; +} + +bool DataSourceBallPresence::isInMouth(){ + return ball->isInFront() && present; +} + +void DataSourceBallPresence::test(){ + DEBUG.print("Reading value: "); + DEBUG.print(CURRENT_DATA_READ.ballPresenceVal); + DEBUG.print(" -> "); + DEBUG.print(CURRENT_DATA_READ.ballPresent); + DEBUG.print(" | Ball in mouth: "); + DEBUG.print(isInMouth()); +} \ No newline at end of file diff --git a/src/sensors/sensors.cpp b/src/sensors/sensors.cpp index b5e4da3..b77f61b 100644 --- a/src/sensors/sensors.cpp +++ b/src/sensors/sensors.cpp @@ -20,6 +20,7 @@ void initSensors(){ // delay(350); bt = new DataSourceBT(&Serial1, 9600); roller = new Roller(30, 31, 1000, 2000, 500); + ballPresence = new DataSourceBallPresence(A22, true); } void updateSensors(){ @@ -30,6 +31,7 @@ void updateSensors(){ compass->update(); ball->update(); + ballPresence->update(); camera->update(); bt->update(); diff --git a/src/test_menu.cpp b/src/test_menu.cpp index bcb566b..15bc19d 100644 --- a/src/test_menu.cpp +++ b/src/test_menu.cpp @@ -110,6 +110,13 @@ void TestMenu::testMenu() while (Serial2.available()) DEBUG.print((char)Serial2.read()); break; + case 'b': + ballPresence->test(); + break; + case 'r': + drive->stopAll(); + flagtest = false; + break; case 's': DEBUG.println("32u4 send Test"); DEBUG.println("Remember LED1 is not used by teensy"); @@ -128,10 +135,6 @@ void TestMenu::testMenu() Serial2.write(0); delay(1500); break; - case 'r': - drive->stopAll(); - flagtest = false; - break; case 'h': default: DEBUG.println("Unknown command, here's a lil help for you :)");