precision shooter,camera: improve, fix math error in translation

alongside with better speed calculation and utility methods

include/motors_movement/drivecontroller.h

@@ -20,7 +20,7 @@
 
 //Max possible vel 310
 
-#define MAX_VEL 120
+#define MAX_VEL 50
 #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_ball_presence.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#define BALL_PRESENCE_TRESH 500
+#define BALL_PRESENCE_TRESH 600
 
 #include <Arduino.h>
 #include "behaviour_control/data_source.h"

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 0
+#define CAMERA_TRANSLATION_X 4
 #define CAMERA_TRANSLATION_Y 7
 
 class DataSourceCameraConic : public DataSource{

include/strategy_roles/precision_shooter.h

@@ -11,7 +11,7 @@
 #define PS_PLUSANG_VISIONCONE 10
 
 // There needs to be a little bit of space between the target point and the spot to be in
-#define PS_SPINNER_OVERHEAD 10
+#define PS_SPINNER_OVERHEAD 6
 
 class PrecisionShooter : public Game{
 
@@ -23,7 +23,7 @@ class PrecisionShooter : public Game{
         void realPlay() override;
         void init() override;
         void catchBall();
-        void spinner();
+        void spinner(int);
         int tilt();
         
     private:

include/systems/position/positionsys_camera.h

@@ -20,9 +20,9 @@
 //Actually it's ± MAX_VAL
 #define MAX_X 50
 #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
-#define DIST_MULT 1.7
+#define DIST_MULT 3.5
 
-#define VICINITY_DIST_TRESH 3
+#define VICINITY_DIST_TRESH 2
 
 #define Kpx 1
 #define Kix 0

src/main.cpp

@@ -57,6 +57,8 @@ void loop() {
 
   testmenu->testMenu();
 
+  // if(roller->roller_armed) roller->speed(roller->MAX);
+
   // Last thing to do: movement and update status vector
   drive->drivePrepared();  
   updateStatusVector();

src/sensors/data_source_ball_presence.cpp

@@ -25,5 +25,5 @@ void DataSourceBallPresence::test(){
     DEBUG.print(" -> ");
     DEBUG.print(CURRENT_DATA_READ.ballPresent);
     DEBUG.print(" | Ball in mouth: ");
-    DEBUG.print(isInMouth());
+    DEBUG.println(isInMouth());
 }

src/sensors/data_source_camera_conicmirror.cpp

@@ -87,10 +87,10 @@ void DataSourceCameraConic ::readSensor() {
 void DataSourceCameraConic ::computeCoordsAngles() {
   //Where are the goals relative to the robot?
   //Remap from [0,100] to [-50, +50] to correctly compute angles and distances and calculate them, getting the original coords calculated by the camera
-  true_xb = 50 - true_xb + CAMERA_TRANSLATION_X;
-  true_yb = true_yb - 50 + CAMERA_TRANSLATION_Y;
-  true_xy = 50 - true_xy + CAMERA_TRANSLATION_X;
-  true_yy = true_yy - 50 + CAMERA_TRANSLATION_Y;
+  true_xb = 50 - true_xb + CAMERA_TRANSLATION_X*0.5;
+  true_yb = true_yb - 50 + CAMERA_TRANSLATION_Y*0.5;
+  true_xy = 50 - true_xy + CAMERA_TRANSLATION_X*0.5;
+  true_yy = true_yy - 50 + CAMERA_TRANSLATION_Y*0.5;
 
   #ifdef CAMERA_CONIC_FILTER_POINTS
   true_xb = filter_xb->calculate(true_xb);

src/sensors/sensors.cpp

@@ -20,7 +20,7 @@ void initSensors(){
     // delay(350);
     bt = new DataSourceBT(&Serial1, 9600);
     roller = new Roller(30, 31, 1000, 2000, 500);
-    ballPresence = new DataSourceBallPresence(A22, true);
+    ballPresence = new DataSourceBallPresence(A13, true);
  }
 
 void updateSensors(){

src/strategy_roles/precision_shooter.cpp

@@ -32,7 +32,7 @@ void PrecisionShooter::init()
 void PrecisionShooter::realPlay()
 {
   if (CURRENT_DATA_READ.ballSeen)
-    this->catchBall();
+    this->spinner(0);
   else
     ps->goCenter();
 }
@@ -51,61 +51,75 @@ unsigned long spinner_timer = 0;
 float spinner_tilt = 0;
 
 void PrecisionShooter::spinner(int targetx){
-  if(!ballPresence->isInMouth()) {
-    spinner_state=0;
-    spinner_flag = false;
-  }
+  // if(!ballPresence->isInMouth()) {
+  //   spinner_state=0;
+  //   spinner_flag = false;
+  // }
 
   if(spinner_state == 0){
-    if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
+    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 > 250) {
+    if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
       spinner_state++;
-      spinner_tilt = CURRENT_DATA_READ.IMUAngle;
+      spinner_flag = false;
     }
   }else if(spinner_state == 1){
-    int spotx;
+    roller->speed(roller->MAX);
 
+    int spotx;
     if(targetx > 0) spotx = targetx-PS_SPINNER_OVERHEAD;
     else spotx = targetx+PS_SPINNER_OVERHEAD;
 
-    ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
-    if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)){
-      ball_catch_state++;
+    if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 5)) {
+
+      if( !spinner_flag){
+        spinner_flag = true;
+        spinner_timer = millis();
+      }
+
+      if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 750) {
+        spinner_state++;
+        spinner_flag = false;
+        spinner_tilt = CURRENT_DATA_READ.IMUAngle;
+      }
 
       if(CURRENT_DATA_READ.posx > targetx){
-        tilt1 = -0.1;
-        tilt2 = 0.35;
+        tilt1 = -0.075;
+        tilt2 = 0.3;
       }else{
-        tilt1 = 0.1;
-        tilt2 = -0.35;
-      }
+        tilt1 = 0.075;
+        tilt2 = -0.3;
       }
+    }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 5);
   }else if(spinner_state == 2){
+    roller->speed(roller->MAX);
+
     spinner_tilt += tilt1;
-    drive(0,0,spinner_tilt);
+    drive->prepareDrive(0,0,spinner_tilt);
     
     if(CURRENT_DATA_READ.IMUAngle > 175 && CURRENT_DATA_READ.IMUAngle < 185) {
       spinner_state++;
       spinner_tilt = CURRENT_DATA_READ.IMUAngle;
+
+      spinner_timer = millis();
     }
     
   }else if(spinner_state == 3){
+    roller->speed(roller->MAX);
+    if(millis() - spinner_timer > 2000) spinner_state++;
+  }else if(spinner_state == 4){
     spinner_tilt += tilt2;
-    drive(0,0,spinner_tilt);
+    drive->prepareDrive(0,0,spinner_tilt);
 
-    if(CURRENT_DATA_READ.IMUAngle > 225 || CURRENT_DATA_READ.IMUAngle < 135){
-      roller->speed(roller->MIN);
-      spinner_state++;
+    if(CURRENT_DATA_READ.IMUAngle > 215 || CURRENT_DATA_READ.IMUAngle < 125) roller->speed(roller->MIN);
+    if(CURRENT_DATA_READ.IMUAngle > 355 || CURRENT_DATA_READ.IMUAngle < 10) spinner_state++;
   }
 }
-
-}
-
 /*
 Ball catch state machine
 0: go towards the ball, until it's been in robot's mouth for 250ms

src/system/positions/positionsys_camera.cpp

@@ -145,10 +145,10 @@ void PositionSysCamera::CameraPID(){
         int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
         dir = (dir+360) % 360;
 
-        int dist = sqrt( ( pow(CURRENT_DATA_WRITE.posx-Setpointx,2) ) + pow(CURRENT_DATA_WRITE.posy-Setpointy, 2) );
-        int speed = dist*DIST_MULT;
-
-        speed = speed > 10 ? speed : 0;
+        // int dist = sqrt( ( pow(CURRENT_DATA_WRITE.posx-Setpointx,2) ) + pow(CURRENT_DATA_WRITE.posy-Setpointy, 2) );
+        // int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL);
+        int speed = hypot(Outputx, Outputy) * DIST_MULT;
+        // speed = speed > 10 ? speed : 0;
         dir = filterDir->calculate(dir);
 
         #ifdef DRIVE_VECTOR_SUM

src/test_menu.cpp

@@ -114,8 +114,8 @@ void TestMenu::testMenu()
             ballPresence->test();
             break;
         case 'r':
-            drive->stopAll();
-            flagtest = false;
+            DEBUG.println("Roller at default speed");
+            roller->speed(roller->MAX);
             break;
         case 's':
             DEBUG.println("32u4 send Test");

utility/OpenMV/conic_eff_h7.py

@@ -46,7 +46,7 @@ blue_led.on()
 
 
 thresholds = [  (70, 100, -36, 8, 38, 112),    # thresholds yellow goalz
-                (50, 77, -23, 8, -60, -21)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (31, 74, -31, 15, -50, -9)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
 roi = (50, 0, 250, 200)
@@ -67,7 +67,7 @@ clock = time.clock()'''
 sensor.reset()
 sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QVGA)
-#sensor.set_windowing(roi)
+sensor.set_windowing(roi)
 sensor.set_contrast(0)
 sensor.set_saturation(2)
 sensor.set_brightness(3)