superteam challenge: st challenge 1

include/sensors/data_source_camera_conicmirror.h

@@ -27,8 +27,8 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/
 // #define CAMERA_TRANSLATION_Y 7
 
 //Robot with roller
-#define CAMERA_TRANSLATION_X -8
+#define CAMERA_TRANSLATION_X 0
-#define CAMERA_TRANSLATION_Y -3
+#define CAMERA_TRANSLATION_Y 12
 
 class DataSourceCameraConic : public DataSource{
 

include/strategy_roles/games.h

@@ -12,6 +12,7 @@
 #include "strategy_roles/striker_roller.h"
 #include "strategy_roles/precision_shooter.h"
 #include "strategy_roles/pass_and_shoot.h"
+#include "strategy_roles/spot_finder.h"
 // #include "strategy_roles/keeper.h"
 
 void initGames();
@@ -23,3 +24,4 @@ g_extr Game* pass_and_shoot;
 // g_extr Game* keeper;
 
 g_extr Game* tc1;
+g_extr Game* tc2;

include/strategy_roles/spot_finder.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include "sensors/data_source_camera_vshapedmirror.h"
+#include "sensors/sensors.h"
+#include "strategy_roles/game.h"
+
+#define X_COORD 10
+#define Y_COORD 15
+
+class SpotFinder : public Game{
+
+    public:
+        SpotFinder();
+        SpotFinder(LineSystem* ls, PositionSystem* ps);
+
+    private:
+        void realPlay() override;
+        void init() override;
+
+        unsigned long t = 0;
+};

include/systems/position/positionsys_camera.h

@@ -23,6 +23,7 @@
 #define DIST_MULT 5
 
 #define VICINITY_DIST_TRESH 2
+#define ROUGH_VICINITY_DIST_TRESH 10
 
 #define Kpx 1
 #define Kix 0
@@ -45,6 +46,7 @@ class PositionSysCamera : public PositionSystem{
         void CameraPID();
         int calcOtherGoalY(int goalY);
         bool isInTheVicinityOf(int, int);
+        bool isInRoughVicinityOf(int, int);
 
         double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
         int MAX_DIST, vx, vy, axisx, axisy;

src/main.cpp

@@ -54,7 +54,7 @@ void loop() {
   // keeper_condition = role == LOW;
 
   if(robot_indentifier){
-    tc1->play(1);
+    tc2->play(1);
     // if(roller->roller_armed) roller->speed(roller->MAX);
     // Last thing to do: movement and update status vector
     drive->drivePrepared();  

src/sensors/data_source_camera_conicmirror.cpp

@@ -201,7 +201,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
   byte to_32u4 = 0;
   to_32u4 |= (CURRENT_DATA_READ.ySeen);
   to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1;
-  BALL_32U4.write(to_32u4);
+  // BALL_32U4.write(to_32u4);
 }
 
 void DataSourceCameraConic::test(){

src/strategy_roles/games.cpp

@@ -17,5 +17,6 @@ void initGames(){
     precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     striker_roller = new StrikerRoller(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     tc1 = new StrikerRoller(new LineSystemEmpty(), new PositionSysCamera());
+    tc2 = new SpotFinder(new LineSystemEmpty(), new PositionSysCamera());
     // keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
 }

src/strategy_roles/spot_finder.cpp

@@ -0,0 +1,33 @@
+#include "strategy_roles/spot_finder.h"
+#include "systems/position/positionsys_camera.h"
+#include "vars.h"
+#include "math.h"
+
+SpotFinder::SpotFinder() : Game() {}
+SpotFinder::SpotFinder(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) {}
+
+void SpotFinder::init() {}
+void SpotFinder::realPlay() {
+  if(millis() - t > 150){
+
+    int zone = 6;
+
+    if(CURRENT_DATA_READ.posx >= -X_COORD && CURRENT_DATA_READ.posx <= X_COORD  && CURRENT_DATA_READ.posy >= -Y_COORD && CURRENT_DATA_READ.posy <= Y_COORD)  BALL_32U4.write(0b000000110); //center
+    else if(CURRENT_DATA_READ.posx <= -X_COORD && CURRENT_DATA_READ.posy <= -Y_COORD) BALL_32U4.write(0b00000100); //bottom left
+    else if(CURRENT_DATA_READ.posx <= -X_COORD && CURRENT_DATA_READ.posy >= Y_COORD) BALL_32U4.write(2); //top left
+    else if(CURRENT_DATA_READ.posx >= X_COORD && CURRENT_DATA_READ.posy <= -Y_COORD) BALL_32U4.write(0b00000001); //bottom right
+    else if(CURRENT_DATA_READ.posx >= X_COORD && CURRENT_DATA_READ.posy >= Y_COORD) BALL_32U4.write(5); //top right
+    else BALL_32U4.write(0);
+
+    // if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(-X_COORD,-Y_COORD) ) BALL_32U4.write(1); //bottom left
+    // else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(-X_COORD,Y_COORD) ) BALL_32U4.write(2); //top left
+    // else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(0,0) ) BALL_32U4.write(3); //center
+    // else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(X_COORD,-Y_COORD) ) BALL_32U4.write(4); //bottom right
+    // else if( ((PositionSysCamera*)ps)->isInRoughVicinityOf(X_COORD,Y_COORD) ) BALL_32U4.write(5); //top right
+
+    t = millis();
+
+
+  }
+  drive->stopAll();
+}

src/system/positions/positionsys_camera.cpp

@@ -53,8 +53,8 @@ void PositionSysCamera::update(){
     posx *= -1;
     posy *= -1;
     
-    //x = 66 is a very very strange bug I can't seem to track down. It's a dirty hack, I know
+    //Filtering error in calculation like this is a dirty hack, I know
-    if(posx == 66 || (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false) ) {
+    if(posx < -MAX_X || posx > MAX_X || posy <  -MAX_Y || posy > MAX_Y || (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false) ) {
         // Go back in time until we found a valid status, when we saw at least one goal
         int i = 1;
         do{
@@ -128,6 +128,11 @@ bool PositionSysCamera::isInTheVicinityOf(int x_, int y_){
     return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= VICINITY_DIST_TRESH*VICINITY_DIST_TRESH;
 }
 
+bool PositionSysCamera::isInRoughVicinityOf(int x_, int y_){
+    // Distance using pytagorean theorem
+    return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= ROUGH_VICINITY_DIST_TRESH*ROUGH_VICINITY_DIST_TRESH;
+}
+
 void PositionSysCamera::CameraPID(){   
     if(givenMovement){
 

utility/OpenMV/conic_eff_h7.py

@@ -45,11 +45,11 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (70, 100, -36, 8, 38, 112),    # thresholds yellow goalz
+thresholds = [  (84, 100, -29, 1, 36, 127),    # thresholds yellow goalz
-                (37, 57, -19, 8, -52, -21)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (36, 100, -34, 12, -59, -19)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (50, 0, 270, 200)
+roi = (70, 0, 250, 200)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx