robocoup: tc2 - round robin

include/behaviour_control/status_vector.h

@@ -36,7 +36,7 @@ typedef struct input{
 }input;
 
 typedef struct data{
-    int IMUAngle, ballAngle, ballAngleFix, ballDistance, ballPresenceVal, 
+    int IMUAngle = 0, IMUOffset = 0, ballAngle = 0, ballAngleFix, ballDistance, ballPresenceVal, 
         yAngle, bAngle, yAngleFix, bAngleFix, 
         yDist, bDist, 
         angleAtk, angleAtkFix, angleDef, angleDefFix, distAtk, distDef, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix,

include/motors_movement/drivecontroller.h

@@ -22,7 +22,7 @@
 
 // #define MAX_POSSIBLE_VEL 310
 #define MAX_POSSIBLE_VEL 280
-#define MAX_VEL 80
+#define MAX_VEL 60
 #define MAX_VEL_EIGTH ((int)MAX_VEL*0.8)
 #define MAX_VEL_HALF ((int)MAX_VEL*0.5)
 #define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75)

include/sensors/data_source_camera_conicmirror.h

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

include/strategy_roles/round_robin.h

@@ -13,12 +13,12 @@
 // There needs to be a little bit of space between the target point and the spot to be in
 #define RR_SPINNER_OVERHEAD 7
 
-#define RR_KICK_LIMIT_TILT1 135
+#define RR_KICK_LIMIT_TILT1 200
-#define RR_KICK_LIMIT_MAX 335
+#define RR_KICK_LIMIT_MAX 315
-#define RR_KICK_LIMIT_MIN 25
+#define RR_KICK_LIMIT_MIN 45
 
 #define ROUND_ROBIN_VEL 30
-#define RR_YCOORD -8
+#define RR_YCOORD -5
 #define RR_ROLLER_SPD 1500
 
 class RoundRobin : public Game{
@@ -32,6 +32,8 @@ class RoundRobin : public Game{
         void init() override;
         void catchBall();
         void spinner(int);
+        void push();
+        void spinnerStep();
         
     private:
         int atk_speed, atk_direction, atk_tilt;
@@ -39,6 +41,8 @@ class RoundRobin : public Game{
         bool gotta_tilt;
         ComplementaryFilter* ballAngleFilter;
 
+        int flip = 0;
+
         float tilt1 = 0;
         float tilt2 = 0;
         int spinner_state = 0;

include/systems/lines/linesys_camera_roundrobin.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <Arduino.h>
+
+#include "behaviour_control/ds_ctrl.h"
+#include "systems/systems.h"
+
+#include "vars.h" 
+
+#define S1O A7
+#define S1I A6
+#define S2O A2
+#define S2I A3
+#define S3I A9
+#define S3O A8
+#define S4I A0
+#define S4O A1
+
+#define LINE_THRESH_CAM 350
+#define EXIT_TIME 300
+
+class LineSysCameraRR : public LineSystem{
+
+    public:
+        LineSysCameraRR();
+        LineSysCameraRR(vector<DataSource*> in_, vector<DataSource*> out);
+
+        void update() override;
+        void test() override;
+        void outOfBounds();
+        void checkLineSensors();
+    
+    public:
+        bool tookLine = false, flag = false;
+        int linetriggerI[4], linetriggerO[4];
+        
+    private:
+        vector<DataSource*> in, out;
+        DataSource* ds;
+        int inV, outV, linesensOldX, linesensOldY, value, linepins[4], i;
+};

src/sensors/data_source_bno055.cpp

@@ -18,6 +18,7 @@ void DataSourceBNO055::readSensor(){
   if(millis() - lastTime > DATA_CLOCK){
     imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
     this->value = (int) euler.x();
+    this->value = (this->value+CURRENT_DATA_READ.IMUOffset+360)%360;
     lastTime = millis();
     CURRENT_INPUT_WRITE.IMUAngle = this->value;
     CURRENT_DATA_WRITE.IMUAngle = this->value;

src/strategy_roles/games.cpp

@@ -2,6 +2,7 @@
 
 /* #include "sensors/linesys_2019.h" */
 #include "systems/lines/linesys_camera.h"
+#include "systems/lines/linesys_camera_roundrobin.h"
 #include "systems/systems.h"
 #include "systems/position/positionsys_zone.h"
 #include "systems/position/positionsys_camera.h"
@@ -17,7 +18,7 @@ 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 RoundRobin(new LineSystemEmpty(), new PositionSysCamera());
+    tc2 = new RoundRobin(new LineSysCameraRR(lIn, lOut), 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/strategy_roles/round_robin.cpp

@@ -1,5 +1,6 @@
 #include "behaviour_control/status_vector.h"
 #include "systems/position/positionsys_camera.h"
+#include "systems/lines/linesys_camera_roundrobin.h"
 #include "sensors/sensors.h"
 #include "sensors/data_source_ball.h"
 #include "strategy_roles/round_robin.h"
@@ -33,6 +34,8 @@ void RoundRobin::realPlay()
 {
   if (CURRENT_DATA_READ.ballSeen)
     this->catchBall();
+    // this->spinner(0);
+    // this->push();
   else{
     ps->goCenter();
     ball_catch_flag = false;
@@ -49,19 +52,16 @@ Spinning kick state machine
 2: tilt back to 0 in the needed direction, stopping the roller whenn needed
 */
 void RoundRobin::spinner(int targetx){
-  // if(!ballPresence->isInMouth()) {
-  //   spinner_state=0;
-  //   spinner_flag = false;
-  // }
 
   if(spinner_state == 0){
-    if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX-200);
+    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 > 100) {
+
+    if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
       spinner_state=1;
       spinner_flag = false;
     }
@@ -72,34 +72,34 @@ void RoundRobin::spinner(int targetx){
     if(targetx >= 0) spotx = targetx-RR_SPINNER_OVERHEAD;
     else spotx = targetx+RR_SPINNER_OVERHEAD;
 
-    if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, RR_YCOORD)) {
+    // if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)) {
-      // if( !spinner_flag){
-      //   spinner_flag = true;
-      //   spinner_timer = millis();
-      // }
 
-      // if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
+    //   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_flag = false;
-        spinner_tilt = CURRENT_DATA_READ.IMUAngle;
+    //     spinner_tilt = CURRENT_DATA_READ.IMUAngle;
-      // }
+    //   }
 
-      if(targetx >= 0) {
+    //   if(targetx >= 0) {
         tilt1 = -0.15;
-        tilt2 = -1.55;
+        tilt2 = -35;
-        limitx = RR_KICK_LIMIT_TILT1;
 
-      }else{
+        limitx = 360-RR_KICK_LIMIT_TILT1;
-        tilt1 = 0.15;
+    //   }else{
-        tilt2 = 1.55;        limitx = 360-RR_KICK_LIMIT_TILT1;
+    //     tilt1 = 0.15;
+    //     tilt2 = -0.55;
 
-
+    //     limitx = RR_KICK_LIMIT_TILT1;
-      }
+    //   }
     
-    }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, RR_YCOORD);
+    // }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
-
   }else if(spinner_state == 2){
-    roller->speed(RR_ROLLER_SPD);
+    roller->speed(roller->MAX);
 
     spinner_tilt += tilt1;
     drive->prepareDrive(0,0,spinner_tilt);
@@ -109,7 +109,7 @@ void RoundRobin::spinner(int targetx){
       spinner_state=3;
     }
   }else if(spinner_state == 3){
-    roller->speed(RR_ROLLER_SPD);
+    roller->speed(roller->MAX);
     drive->prepareDrive(0,0,spinner_tilt);
     if(millis() - spinner_timer > 150) {
       spinner_state=4;
@@ -117,17 +117,145 @@ void RoundRobin::spinner(int targetx){
     }
   }else if(spinner_state == 4){
     // drive->prepareDrive(0,0,0);
-
     spinner_tilt += tilt2;
     spinner_tilt = constrain(spinner_tilt, RR_KICK_LIMIT_MIN, RR_KICK_LIMIT_MAX);
     drive->prepareDrive(0,0,spinner_tilt);
 
     if(CURRENT_DATA_READ.IMUAngle >= RR_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= RR_KICK_LIMIT_MIN) {
-      roller->speed(roller->MIN);
+    ball_catch_state++;
-      spinner_tilt = 0;
+
+    roller->speed(roller->MIN);
     }
   }
 }
+
+
+void RoundRobin::spinnerStep(){
+  spinner_state++;
+  spinner_timer = millis();
+}
+
+/*void RoundRobin::push(){
+  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);
+
+    if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -6)) spinner_state=4;
+    else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -6);
+  // } else if(spinner_state == 2){
+  //   roller->speed(roller->MAX);
+  //   if(((PositionSysCamera*)ps)->isInTheVicinityOf(16, -4)) spinnerStep();
+  //   else ((PositionSysCamera*)ps)->setMoveSetpoints(16, -4);
+  // }else if(spinner_state == 3){
+  //   drive->prepareDrive(0,0,0);
+  //   if(millis() - spinner_timer > 750) spinnerStep();
+  }else if(spinner_state == 4){
+    drive->prepareDrive(45, 50, 0);
+    if( ((LineSysCameraRR*)ls)->tookLine ) spinnerStep();
+  }else if(spinner_state == 5){
+    roller->speed(roller->MIN);
+    if(millis()-spinner_timer > 1500) spinnerStep();
+  }else if(spinner_state == 6){
+    drive->prepareDrive(180, 30, 0);
+    if(millis() - spinner_timer > 750) spinnerStep();
+  }else if(spinner_state == 7){
+    roller->speed(roller->MIN);
+    drive->prepareDrive(0, 0, 55);
+    if(millis() - spinner_timer > 1000) spinnerStep();
+  }else if(spinner_state == 8){
+    drive->prepareDrive(drive->directionAccountingForTilt(0, 55), 100, 55);
+    if(millis() - spinner_timer > 500) spinnerStep();
+  }else if(spinner_state == 9){
+    drive->prepareDrive(drive->directionAccountingForTilt(180, 55), 50, 55);
+    if(millis() - spinner_timer > 750) spinnerStep();
+  }else if(spinner_state == 10){
+    if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -8)) spinnerStep();
+    else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -8);
+  }else ball_catch_state++;
+}*/
+void RoundRobin::push(){
+  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);
+
+    // if(flip)
+    //   if(CURRENT_DATA_READ.yDist <= 24) drive->prepareDrive(0, 50, 0);
+    //   if(CURRENT_DATA_READ.yDist >= 36)  drive->prepareDrive(180, 50, 0);
+    // else{
+    //   if(CURRENT_DATA_READ.bDist <= 24) drive->prepareDrive(0, 50, 0);
+    //   if(CURRENT_DATA_READ.bDist >= 36)  drive->prepareDrive(180, 50, 0);
+    // }
+
+    if(flip==0){
+    if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -3)) spinnerStep();
+    else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -3);
+    }else{
+    if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -12)) spinnerStep();
+    else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -12);
+    }
+  } else if(spinner_state == 2){
+  //   roller->speed(roller->MAX);
+  //   if(((PositionSysCamera*)ps)->isInTheVicinityOf(16, -4)) spinnerStep();
+  //   else ((PositionSysCamera*)ps)->setMoveSetpoints(16, -4);
+  // }else if(spinner_state == 3){
+  //   drive->prepareDrive(0,0,0);
+  //   if(millis() - spinner_timer > 750) spinnerStep();
+  // }else if(spinner_state == 4){
+    // drive->prepareDrive(45, 50, 0);
+    drive->prepareDrive(70, 40, 0);
+    if( ((LineSysCameraRR*)ls)->tookLine ) {
+      spinner_state = 5;
+      spinner_timer  = millis();
+    }
+  }else if(spinner_state == 5){
+    roller->speed(roller->MIN);
+    if(millis()-spinner_timer > 1500) spinnerStep();
+  }else if(spinner_state == 6){
+    drive->prepareDrive(180, 30, 0);
+    if(millis() - spinner_timer > 650) spinnerStep();
+  }else if(spinner_state == 7){
+    roller->speed(roller->MIN);
+    drive->prepareDrive(0, 0, 55);
+    if(millis() - spinner_timer > 1000) spinnerStep();
+  }else if(spinner_state == 8){
+    drive->prepareDrive(drive->directionAccountingForTilt(0, 90), 100, 90);
+    if(millis() - spinner_timer > 400) spinnerStep();
+  }else if(spinner_state == 9){
+    drive->prepareDrive(drive->directionAccountingForTilt(180, 90), 50, 90);
+    if(millis() - spinner_timer > 750) spinnerStep();
+  }else if(spinner_state == 10){
+    if(flip==0){
+    if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -4)) spinnerStep();
+    else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -4);
+    }else{
+    if(((PositionSysCamera*)ps)->isInRoughVicinityOf(0, -13)) spinnerStep();
+    else ((PositionSysCamera*)ps)->setMoveSetpoints(0, -13);
+    }
+  }else ball_catch_state++;
+}
+
 /*
 Ball catch state machine
 0: go towards the ball, until it's been in robot's mouth for 250ms
@@ -137,13 +265,14 @@ Ball catch state machine
 
 void RoundRobin::catchBall(){
 
-  if(!ball->isInFront()){
+  // if(!ball->isInFront() && ball_catch_state != 3){
-    ball_catch_state = 0;
+  //   ball_catch_state = 0;
-    ball_catch_flag = false;
+  //   ball_catch_flag = false;
-    ball_catch_tilt = 0;
+  //   ball_catch_tilt = 0;
-  }
+  // }
 
   if(ball_catch_state == 0){
+    ((LineSysCameraRR*)ls)->flag = true;
 
     if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
     else roller->speed(roller->MIN);
@@ -172,7 +301,24 @@ void RoundRobin::catchBall(){
     spinner_timer = 0;
     spinner_state = 1;
     ball_catch_state = 3;
+
+    ((LineSysCameraRR*)ls)->flag = false;
   }else if(ball_catch_state == 3){
-    this->spinner(28);
+    // this->spinner(25);
-  }
+    this->push();
+  }else if(ball_catch_state == 4){
+    drive->prepareDrive(270, 50, 0);
+    if(((LineSysCameraRR*)ls)->tookLine && CURRENT_DATA_READ.posx <= -10) ball_catch_state = 5;
+  }else if(ball_catch_state == 5){
+    if( ((LineSysCameraRR*)ls)->linetriggerI[1] || ((LineSysCameraRR*)ls)->linetriggerO[1] > 0) drive->prepareDrive(90, 50, 0); //with 2 and 4 you go right or left accordingly
+    else if( ((LineSysCameraRR*)ls)->linetriggerI[3] || ((LineSysCameraRR*)ls)->linetriggerO[3] > 0 ) drive->prepareDrive(270, 50, 0);
+    else drive->prepareDrive(0, 30, 0);
+    if(CURRENT_DATA_READ.posy > 7) {
+      ball_catch_state=0;
+      
+      flip = 1-flip;
+      
+      CURRENT_DATA_WRITE.IMUOffset = 180 * flip;
+    }
+  }else if(ball_catch_state==6) drive->prepareDrive(0,0,0);
 }

src/system/lines/linesys_camera_roundrobin.cpp

@@ -0,0 +1,77 @@
+#include "systems/lines/linesys_camera_roundrobin.h"
+#include "systems/position/positionsys_camera.h"
+#include "sensors/sensors.h"
+#include "strategy_roles/games.h"
+#include "behaviour_control/status_vector.h"
+
+LineSysCameraRR::LineSysCameraRR(){}
+LineSysCameraRR::LineSysCameraRR(vector<DataSource*> in_, vector<DataSource*> out_){
+    this->in = in_;
+    this->out = out_;
+
+    linesensOldX = 0;
+    linesensOldY = 0;
+
+    tookLine = false;
+
+    for(int i = 0; i < 4; i++){
+      linetriggerI[i] = 0;
+      linetriggerO[i] = 0;
+    }
+}
+
+
+void LineSysCameraRR ::update() {
+  inV = 0;
+  outV = 0;
+  tookLine = false;
+
+  for(DataSource* d : in) d->readSensor();
+  for(DataSource* d : out) d->readSensor();
+
+  for(auto it = in.begin(); it != in.end(); it++){
+    i = it - in.begin();
+    ds = *it;
+    linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM;
+  }
+  for(auto it = out.begin(); it != out.end(); it++){
+    i = it - out.begin();
+    ds = *it;
+    linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM;
+  }
+
+  for(int i = 0; i < 4; i++){
+    inV = inV | (linetriggerI[i] << i);
+    outV = outV | (linetriggerO[i] << i);
+  }
+
+  tookLine = inV > 0 || outV > 0;
+  // if(flag && tookLine) ((PositionSysCamera*)CURRENT_DATA_READ.posSystem)->setMoveSetpoints(0, -8);
+}
+
+void LineSysCameraRR::test(){
+  update();
+  DEBUG.print("In: ");
+  for(DataSource* d : in){
+      d->update();
+      DEBUG.print(d->getValue());
+      DEBUG.print(" | ");
+  }
+  DEBUG.print(" |---| ");
+  DEBUG.print("Out: ");
+  for(DataSource* d : out){
+      d->update();
+      DEBUG.print(d->getValue());
+      DEBUG.print(" | ");
+  }
+  DEBUG.println();
+  for(int i = 0; i < 4; i++){
+    DEBUG.print(linetriggerI[i]);
+    DEBUG.print(" | ");
+    DEBUG.println(linetriggerO[i]);
+  }
+
+  DEBUG.println(inV);
+  DEBUG.println(outV);
+  DEBUG.println();
+}

src/system/positions/positionsys_camera.cpp

@@ -152,7 +152,7 @@ void PositionSysCamera::CameraPID(){
         dir = (dir+360) % 360;
 
         float distance = hypot(Outputx, Outputy);
-        float speed = distance > 3 ? 20 + map(distance, 0, MAX_DIST_EXPERIMENTAL, 0, MAX_POSSIBLE_VEL) : 0;
+        float speed = distance > 2 ? 20 + map(distance, 0, MAX_DIST_EXPERIMENTAL, 0, 200) : 0;
 
         // DEBUG.print("x: ");
         // DEBUG.print(Outputx);

utility/OpenMV/conic_eff_h7.py

@@ -45,11 +45,11 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (68, 100, -16, 26, 24, 85),    # thresholds yellow goalz
+thresholds = [  (62, 100, -10, 21, 27, 112),    # thresholds yellow goalz
-                (36, 70, -3, 27, -75, -27)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (34, 60, -4, 18, -65, -19)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (70, 0, 250, 200)
+roi = (50, 0, 270, 200)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx
@@ -71,7 +71,7 @@ sensor.set_windowing(roi)
 sensor.set_contrast(0)
 sensor.set_saturation(2)
 sensor.set_brightness(3)
-sensor.set_auto_whitebal(True, (-6.02073, -5.119987, -1.006964))
+sensor.set_auto_whitebal(False, (-6.02073, -4.99849, -1.083474))
 sensor.set_auto_exposure(False, 6576)
 #sensor.set_auto_gain(False, gain_db=8.78)
 sensor.skip_frames(time = 300)