robocup: tc3: corner kick

Co-authored-by: u-siri-ous <sanninosiria@gmail.com>

include/sensors/data_source_bt.h

@@ -13,7 +13,7 @@ class DataSourceBT : public DataSource{
         void receive();
         void send();
 
-        bool can_bombard, bt_bombarded, comrade;
+        bool can_send, bt_bombarded, comrade;
         unsigned long bt_timer, last_received, t;
 
         char received, tosend;

include/strategy_roles/corner_kick_1.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "systems/position/positionsys_camera.h"
+#include "systems/lines/linesys_camera.h"
+#include "sensors/sensors.h"
+#include "strategy_roles/game.h"
+#include "strategy_roles/games.h"
+#include "behaviour_control/status_vector.h"
+#include "sensors/data_source_ball.h"
+#include "strategy_roles/striker.h"
+#include "sensors/data_source_ball_presence.h"
+#include "sensors/data_source_bt.h"
+#include "vars.h"
+
+class CornerKick : public Game{
+    public:
+        CornerKick();
+        CornerKick(LineSystem* ls, PositionSystem* ps);
+
+        void init() override;
+        void realPlay() override;
+        void kick();
+        unsigned long kicktimer = 0, debounce_timer = 0;
+        int kick_state = 99;
+        bool kick_flag = false;
+
+        int old_btn = 0, new_val = 0;
+        ComplementaryFilter* ballAngleFilter = new ComplementaryFilter(0.85);
+};

include/strategy_roles/corner_kick_2.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "sensors/data_source_camera_vshapedmirror.h"
+#include "sensors/sensors.h"
+#include "strategy_roles/game.h"
+
+
+#define CK2_SPINNER_OVERHEAD 10
+
+#define CK2_KICK_LIMIT_TILT1 200
+#define CK2_KICK_LIMIT_MAX 315
+#define CK2_KICK_LIMIT_MIN 45
+
+class CornerKick2 : public Game{
+
+    public:
+        CornerKick2();
+        CornerKick2(LineSystem* ls, PositionSystem* ps);
+
+    private:
+        void realPlay() override;
+        void init() override;
+
+        void catchBall();
+        void spinner(int);
+
+        unsigned long timer = 0;
+        int state = 0;
+
+        int ball_catch_state = 0;
+        int ball_catch_timer = 0;
+        bool ball_catch_flag = false;
+        float ball_catch_tilt = 0;
+
+        int goal_coords = 0;
+
+        ComplementaryFilter* ballAngleFilter = new ComplementaryFilter(0.85);
+
+        int limitx = 0, spinner_state = 0;
+        bool spinner_flag = false;
+        float tilt1 = 0, tilt2 = 0, spinner_tilt = 0;
+        unsigned long spinner_timer = 0;
+};

include/strategy_roles/games.h

@@ -16,6 +16,7 @@
 #include "strategy_roles/the_spinner.h"
 #include "strategy_roles/round_robin.h"
 #include "strategy_roles/corner_kick_2.h"
+#include "strategy_roles/corner_kick_1.h"
 // #include "strategy_roles/keeper.h"
 
 void initGames();
@@ -30,4 +31,5 @@ g_extr Game* tc1;
 g_extr Game* tc2;
 g_extr Game* st_tc1;
 g_extr Game* st_tc3;
+g_extr Game* tc3_1;
 g_extr Game* tc3_2;

include/systems/position/positionsys_camera.h

@@ -48,6 +48,7 @@ class PositionSysCamera : public PositionSystem{
         int calcOtherGoalY(int goalY);
         bool isInTheVicinityOf(int, int);
         bool isInRoughVicinityOf(int, int);
+        bool isAtDistanceFrom(int, int, int);
 
         double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
         int MAX_DIST, vx, vy, axisx, axisy;

src/main.cpp

@@ -54,7 +54,9 @@ void loop() {
   // keeper_condition = role == LOW;
 
   if(robot_indentifier){
-    tc2->play(1);
+    tc3_2->play(role);
+    tc3_1->play(!role);
+
     // Last thing to do: movement and update status vector
     drive->drivePrepared();  
   }else{

src/sensors/data_source_bt.cpp

@@ -21,8 +21,8 @@ For now it seems that a slave can easily recovery from a master restart, but not
 
 DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_, baud){
     bt_timer = millis();
-    can_bombard = false;
     bt_bombarded = false;
+    can_send = false;
 
     comrade = false;
 
@@ -64,7 +64,7 @@ void DataSourceBT :: receive(){
 }
 
 void DataSourceBT::send(){
-  if(millis() - t >= 250){
+  if(millis() - t >= 250 && can_send ){
     Serial1.print(tosend);
   }
 }
@@ -73,7 +73,7 @@ void DataSourceBT::update(){
   // if(!bt_bombarded && can_bombard) connect();
   receive();
   send();
-  // if(comrade)Serial2.write(0b00000100);
+  if(comrade)Serial2.write(0b00000100);
 }
 
 void DataSourceBT :: test(){

src/strategy_roles/corner_kick_1.cpp

@@ -0,0 +1,83 @@
+#include "strategy_roles/corner_kick_1.h"
+#include <Bounce2.h>
+
+CornerKick::CornerKick() : Game(){
+    init();
+}
+
+CornerKick::CornerKick(LineSystem *ls_, PositionSystem *ps_) : Game(ls_,ps_){
+  init();
+} 
+
+Bounce b = Bounce(); // Instantiate a Bounce object
+
+void CornerKick::init(){
+  b.attach(37, INPUT);
+  b.interval(25);
+}
+
+void CornerKick::realPlay(){
+  b.update();
+    if(CURRENT_DATA_READ.ballSeen){
+      if(b.fell()) kick_state = 0;
+      this->kick();
+    }
+    else drive->prepareDrive(0,0,0);
+}
+
+void CornerKick::kick(){
+    //DEBUG.println(kick_state);
+  if(kick_state == 0){
+    kicktimer = millis();
+    kick_state++;
+    kick_flag = false;
+  } else if(kick_state == 1){
+    drive->prepareDrive(0, 0, CURRENT_DATA_READ.ballAngleFix);
+    if((CURRENT_DATA_READ.ballAngle >= 350 || CURRENT_DATA_READ.ballAngle <= 10) && millis() - kicktimer > 250) kick_state ++;
+  } else if(kick_state==2){
+    drive->prepareDrive(0, 60, CURRENT_DATA_READ.ballAngleFix);
+    if(ball->isInMouth()){
+      kick_state++;
+      if(!kick_flag) {
+        kick_flag = true;
+        kicktimer = millis();
+      }else{
+        if(kick_flag && millis() - kicktimer > 700){
+          kick_flag = false;
+          kick_state++;
+          kicktimer = millis();
+        }
+      }
+    }
+  } else if(kick_state==3){
+    drive->prepareDrive(0, 150, CURRENT_DATA_READ.ballAngleFix);
+    if(millis()-kicktimer > 400){
+        kick_state++;
+    }
+  } else if(kick_state==4){
+    if(((PositionSysCamera*)ps)->isAtDistanceFrom(0, -28, 5)) {
+      kick_state++;
+      kicktimer = millis();
+    }else (((PositionSysCamera*)ps)->setMoveSetpoints(0, -28));
+  } else if(kick_state == 5){
+    drive->prepareDrive(0,0,0);
+    bt->can_send = true;
+    bt->tosend = 'C';
+    if(millis() - kicktimer > 500){
+      kick_state++;
+      tone(BUZZER, 220, 250);
+      kicktimer = millis();
+    }
+  } else if(kick_state == 6){
+    bt->can_send = true;
+    bt->tosend = 'B';
+    if(millis() - kicktimer > 750){
+      kick_state++;
+      kicktimer = millis();
+      bt->can_send = false;
+  }
+  }else if (kick_state == 7) {
+    bt->can_send = false;
+    drive->prepareDrive(0,0,0);
+  }
+}

src/strategy_roles/corner_kick_2.cpp

@@ -0,0 +1,148 @@
+#include "strategy_roles/corner_kick_2.h"
+#include "systems/position/positionsys_camera.h"
+#include "vars.h"
+#include "math.h"
+
+CornerKick2::CornerKick2() : Game() {}
+CornerKick2::CornerKick2(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) {}
+
+void CornerKick2::init() {}
+
+void CornerKick2::realPlay() {
+  if(bt->received == 'C') {
+    bt->can_send = true;
+    bt->tosend = 'B';
+    bt->can_send = false;
+
+    tone(BUZZER, 320, 250);
+    timer = millis();
+    state=1;
+  }
+  if(state == 1){
+    drive->prepareDrive(265, 50, 0);
+    if(CURRENT_DATA_READ.bSeen && millis() - timer > 500) {
+      state ++;
+      timer = millis();
+    }
+  }else if(state == 2){
+    drive->prepareDrive(315, 50, 0);
+    if(millis() - timer > 200) state++;
+  }else if(state == 3){
+    catchBall();
+  }else if(state == 4){
+    spinner(0);
+  }else if(state == 5){
+    drive->stopAll();
+  }
+}
+
+void CornerKick2::catchBall(){
+
+  if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
+  else roller->speed(roller->MIN);
+
+  if(!ball->isInFront()){
+    ball_catch_state = 0;
+    ball_catch_flag = false;
+  }
+
+  if(ball_catch_state == 0){
+    drive->prepareDrive(0, MAX_VEL_HALF, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
+
+    if(ballPresence->isInMouth() && !ball_catch_flag){
+      ball_catch_flag = true;
+      ball_catch_timer = millis();
+    }
+
+    if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 350) {
+      ball_catch_state++;
+      ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
+    }
+  }else if(ball_catch_state == 1){
+    if(ball_catch_tilt > 180) ball_catch_tilt += 0.01;
+    else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.01;
+
+    drive->prepareDrive(0,0,ball_catch_tilt);
+
+    if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) state++;
+    // ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0);
+    // if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++;
+  }
+}
+
+void CornerKick2::spinner(int targetx){
+  if(spinner_state == 0){
+    if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
+
+    if(ballPresence->isInMouth() && !spinner_flag){
+      spinner_flag = true;
+      spinner_timer = millis();
+    }
+
+    if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
+      spinner_state=1;
+      spinner_flag = false;
+    }
+  }else if(spinner_state == 1){
+    roller->speed(roller->MAX);
+
+    int spotx = targetx;
+    // if(targetx >= 0) spotx = targetx-CK2_SPINNER_OVERHEAD;
+    // else spotx = targetx+CK2_SPINNER_OVERHEAD;
+
+    if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)) {
+
+      if( !spinner_flag){
+        spinner_flag = true;
+        spinner_timer = millis();
+      }
+
+      if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
+        spinner_state=2;
+        spinner_flag = false;
+        spinner_tilt = CURRENT_DATA_READ.IMUAngle;
+      }
+
+      if(targetx >= 0) {
+        tilt1 = -0.01;
+        tilt2 = 0.55;
+
+        limitx = 360-CK2_KICK_LIMIT_TILT1;
+      }else{
+        tilt1 = 0.01;
+        tilt2 = -0.55;
+
+        limitx = CK2_KICK_LIMIT_TILT1;
+      }
+    
+    }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
+  }else if(spinner_state == 2){
+    roller->speed(roller->MAX);
+
+    spinner_tilt += tilt1;
+    drive->prepareDrive(0,0,spinner_tilt);
+    
+    if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) {
+      spinner_timer = millis();
+      spinner_state=3;
+    }
+  }else if(spinner_state == 3){
+    roller->speed(roller->MAX);
+    drive->prepareDrive(0,0,spinner_tilt);
+    if(millis() - spinner_timer > 150) {
+      spinner_state=4;
+      spinner_tilt = CURRENT_DATA_READ.IMUAngle;
+    }
+  }else if(spinner_state == 4){
+    drive->prepareDrive(0,0,0);
+
+    // spinner_tilt += tilt2;
+    // spinner_tilt = constrain(spinner_tilt, CK2_KICK_LIMIT_MIN, CK2_KICK_LIMIT_MAX);
+    // drive->prepareDrive(0,0,spinner_tilt);
+
+    if(CURRENT_DATA_READ.IMUAngle >= CK2_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= CK2_KICK_LIMIT_MIN) {
+      roller->speed(roller->MIN);
+      state++;
+    }
+  }
+}

src/strategy_roles/games.cpp

@@ -17,8 +17,13 @@ void initGames(){
     pass_and_shoot = new PassAndShoot(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     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 RoundRobin(new LineSysCameraRR(lIn, lOut), new PositionSysCamera());
+    
+    tc3_1 = new CornerKick(new LineSystemEmpty(), new PositionSysCamera());
+    tc3_2 = new CornerKick2(new LineSystemEmpty(), new PositionSysCamera());
+    
     st_tc1 = new SpotFinder(new LineSystemEmpty(), new PositionSysCamera());
     st_tc3 = new Spinner(new LineSystemEmpty(), new PositionSysCamera());
     // keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());

src/system/positions/positionsys_camera.cpp

@@ -132,6 +132,11 @@ bool PositionSysCamera::isInRoughVicinityOf(int x_, int y_){
     return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= ROUGH_VICINITY_DIST_TRESH*ROUGH_VICINITY_DIST_TRESH;
 }
 
+bool PositionSysCamera::isAtDistanceFrom(int x_, int y_, int dist){
+    // Distance using pytagorean theorem
+    return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= dist*dist;
+}
+
 void PositionSysCamera::CameraPID(){   
     if(givenMovement){