From f26cbf4984ab80dc710f4b68c4b2c6c93de69ee3 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Mon, 22 Mar 2021 14:03:53 +0100 Subject: [PATCH] movement: enable vector sum for multiple movements in a single cycle --- include/motors_movement/drivecontroller.h | 10 ++--- include/strategy_roles/keeper.h | 16 ++++++- include/systems/lines/linesys_camera.h | 2 +- include/systems/position/positionsys_camera.h | 15 ++++--- src/main.cpp | 11 +++-- src/motors_movement/drivecontroller.cpp | 45 +++++++++++++++---- src/strategy_roles/keeper.cpp | 32 ++++++++----- src/system/positions/positionsys_camera.cpp | 4 +- utility/OpenMV/conic_eff_h7.py | 4 +- 9 files changed, 98 insertions(+), 41 deletions(-) diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index d3793e2..1dce17b 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -18,11 +18,11 @@ #define UNLOCK_THRESH 800 -#define MAX_VEL 310 -#define MAX_VEL_EIGTH 248 -#define MAX_VEL_HALF 155 -#define MAX_VEL_3QUARTERS 232 -#define MAX_VEL_QUARTER 78 +#define MAX_VEL 150 +#define MAX_VEL_EIGTH 120 +#define MAX_VEL_HALF 75 +#define MAX_VEL_3QUARTERS 112 +#define MAX_VEL_QUARTER 38 class DriveController{ diff --git a/include/strategy_roles/keeper.h b/include/strategy_roles/keeper.h index 1245dfd..33d866c 100644 --- a/include/strategy_roles/keeper.h +++ b/include/strategy_roles/keeper.h @@ -1,14 +1,28 @@ #pragma once #include "strategy_roles/game.h" +#include "systems/position/positionsys_camera.h" -#define KEEPER_N_POINTS 3 +#define KEEPER_3_POINTS +//#define KEEPER_5_POINTS +#ifdef KEEPER_3_POINTS +#define KEEPER_POINT_LEFT -1 +#define KEEPER_POINT_CENTER 0 +#define KEEPER_POINT_RIGHT 1 +#define KEEPER_POINT_LEFT_C CAMERA_GOAL_MIN_X +#define KEEPER_POINT_CENTER_C CAMERA_GOAL_X +#define KEEPER_POINT_RIGHT_C CAMERA_GOAL_MAX_X +#endif +#define KEEPER_ATTACK_DISTANCE 120 class Keeper : public Game{ public: Keeper(); Keeper(LineSystem*, PositionSystem*); + + public: + bool shouldStrike; private: void realPlay() override; diff --git a/include/systems/lines/linesys_camera.h b/include/systems/lines/linesys_camera.h index 3edabef..d0ba408 100644 --- a/include/systems/lines/linesys_camera.h +++ b/include/systems/lines/linesys_camera.h @@ -16,7 +16,7 @@ #define S4I A9 #define S4O A8 -#define LINE_THRESH_CAM 600 +#define LINE_THRESH_CAM 300 #define EXIT_TIME 250 #define LINES_EXIT_SPD 350 diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index d8f662e..65ff45b 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -1,29 +1,30 @@ -#include "PID_v2.h" +#pragma once +#include "PID_v2.h" #include "systems/systems.h" /*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time. These values need to be subtracted from the coords used in setMoveSetpoints*/ #define CAMERA_TRANSLATION_X 0 -#define CAMERA_TRANSLATION_Y 12 +#define CAMERA_TRANSLATION_Y 3 //Camera center: those setpoints correspond to what we consider the center of the field #define CAMERA_CENTER_X 0 #define CAMERA_CENTER_Y 0 //Camera goal: those setpoints correspond to the position of the center of the goal on the field -#define CAMERA_GOAL_X 10 -#define CAMERA_GOAL_Y 0 +#define CAMERA_GOAL_X 0 +#define CAMERA_GOAL_Y -12 -#define CAMERA_GOAL_MIN_X -15 -#define CAMERA_GOAL_MAX_X 35 +#define CAMERA_GOAL_MIN_X -14 +#define CAMERA_GOAL_MAX_X 14 #define CAMERA_CENTER_Y_ABS_SUM 60 //Actually it's ± MAX_VAL #define MAX_X 50 #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2) -#define DIST_MULT 1.65 +#define DIST_MULT 1.4 #define Kpx 1 #define Kix 0 diff --git a/src/main.cpp b/src/main.cpp index 94c97fb..4bdce9c 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,6 +10,9 @@ TestMenu* testmenu; +bool striker_condition = false; +bool keeper_condition = false; + void setup() { delay(1500); DEBUG.begin(9600); @@ -37,11 +40,11 @@ void loop() { updateSensors(); if(DEBUG.available()) testmenu->testMenu(); - // drive->prepareDrive(0,0,0); + striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike; + keeper_condition = role == LOW; - striker->play(1); - // striker_test->play(1); - // keeper->play(role==0); + striker->play(striker_condition); + keeper->play(keeper_condition); // Last thing to do: movement and update status vector drive->drivePrepared(); diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index 21f12b9..ef8816b 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -70,18 +70,19 @@ void DriveController::drive(int dir, int speed, int tilt){ // Re enabling the below lines requires to comment out drive->prepareDrive and uncommenting the lines relative to vector sum inside positionsys_camera and comment out the other lines here // vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx; // vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy; + vx = ((speed * cosins[dir])); vy = ((-speed * sins[dir])); - if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) { - vxn = 0; - vxp = 0; - vyp = 0; - vyn = 0; - } + // if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) { + // vxn = 0; + // vxp = 0; + // vyp = 0; + // vyn = 0; + // } - if((vy > 0 && vxn == 1) || (vy < 0 && vxp == 1)) vy = 0; - if((vx > 0 && vyp == 1) || (vx < 0 && vyn == 1)) vx = 0; + // if((vy > 0 && vxn == 1) || (vy < 0 && vxp == 1)) vy = 0; + // if((vx > 0 && vyp == 1) || (vx < 0 && vyn == 1)) vx = 0; speed1 = ((vx * sins[m1->angle] ) + (vy * cosins[m1->angle] )); speed2 = ((vx * sins[m2->angle]) + (vy * cosins[m2->angle])); @@ -103,6 +104,34 @@ void DriveController::drive(int dir, int speed, int tilt){ speed3 += pidfactor; speed4 += pidfactor; + // Find the maximum speed and scale all of them for the maximum to be 255 + float maxVel = 0; + maxVel = max(abs(speed1), maxVel); + maxVel = max(abs(speed2), maxVel); + maxVel = max(abs(speed3), maxVel); + maxVel = max(abs(speed4), maxVel); + + if(maxVel > 255){ + // Ratio to 255 + float ratio = maxVel/255; + + // //Scale all the velocities + speed1 /= ratio; + speed2 /= ratio; + speed3 /= ratio; + speed4 /= ratio; + + DEBUG.print(speed1); + DEBUG.print(" | "); + DEBUG.print(speed2); + DEBUG.print(" | "); + DEBUG.print(speed3); + DEBUG.print(" | "); + DEBUG.print(speed4); + DEBUG.print(" | "); + DEBUG.println(maxVel); + } + speed1 = constrain(speed1, -255, 255); speed2 = constrain(speed2, -255, 255); speed3 = constrain(speed3, -255, 255); diff --git a/src/strategy_roles/keeper.cpp b/src/strategy_roles/keeper.cpp index 550af15..1f351ad 100644 --- a/src/strategy_roles/keeper.cpp +++ b/src/strategy_roles/keeper.cpp @@ -7,6 +7,8 @@ #include "systems/position/positionsys_camera.h" #include +int currentPosition = 0; + Keeper::Keeper() : Game() { init(); @@ -18,7 +20,7 @@ Keeper::Keeper(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) void Keeper::init() { - point_spacing = (abs(CAMERA_GOAL_MIN_X) + abs(CAMERA_GOAL_MAX_X)) / KEEPER_N_POINTS; + shouldStrike = false; } void Keeper::realPlay() @@ -31,15 +33,23 @@ void Keeper::realPlay() void Keeper::keeper() { - //Convert Ball position into a coordinate in the Camera Position Sys plane - float ball_x = cos((-90 + CURRENT_DATA_READ.ballAngle) * 3.14 / 180); - // Remap between GOAL positions - ball_x = (int)map(ball_x, -1, 1, CAMERA_GOAL_MIN_X, CAMERA_GOAL_MAX_X); + shouldStrike = false; - // for (int i = CAMERA_GOAL_MIN_X; i <= CAMERA_GOAL_MAX_X; i += point_spacing) - // if (ball_x < i) - // { - ((PositionSysCamera *)ps)->setMoveSetpoints(ball_x, CAMERA_GOAL_Y); - // break; - // } + if(CURRENT_DATA_READ.ballDistance < KEEPER_ATTACK_DISTANCE || (CURRENT_DATA_READ.ballAngle >= 90 && CURRENT_DATA_READ.ballAngle <= 270)){ + shouldStrike = true; + return; + } + + if(CURRENT_DATA_READ.ballAngle >= 330 || CURRENT_DATA_READ.ballAngle <= 30) currentPosition = currentPosition; //Unneeded, just here for clarity + else if(CURRENT_DATA_READ.ballAngle > 30 && CURRENT_DATA_READ.ballAngle < 90) currentPosition ++; + else if(CURRENT_DATA_READ.ballAngle > 270 && CURRENT_DATA_READ.ballAngle < 330) currentPosition --; + else{ + shouldStrike = true; + } + + currentPosition = constrain(currentPosition, KEEPER_POINT_LEFT, KEEPER_POINT_RIGHT); + + if(currentPosition == KEEPER_POINT_LEFT) ((PositionSysCamera*)ps)->setMoveSetpoints(KEEPER_POINT_LEFT_C, CAMERA_GOAL_Y); + else if(currentPosition == KEEPER_POINT_CENTER) ((PositionSysCamera*)ps)->setMoveSetpoints(KEEPER_POINT_CENTER_C, CAMERA_GOAL_Y); + else if(currentPosition == KEEPER_POINT_RIGHT) ((PositionSysCamera*)ps)->setMoveSetpoints(KEEPER_POINT_RIGHT_C, CAMERA_GOAL_Y); } diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index a6abd8c..da249d5 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -77,16 +77,15 @@ void PositionSysCamera::addMoveOnAxis(int x, int y){ axisx += x; axisy += y; givenMovement = true; + CameraPID(); } void PositionSysCamera::goCenter(){ setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y); - CameraPID(); } void PositionSysCamera::centerGoal(){ setMoveSetpoints(CAMERA_GOAL_X, CAMERA_GOAL_Y); - CameraPID(); } /*Knowing the sum of the absolute values of the y position of the goals, it calculates the missing goal y knowing the other one @@ -119,6 +118,7 @@ void PositionSysCamera::CameraPID(){ int dist = sqrt(Outputx*Outputx + Outputy*Outputy); int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL); + speed = speed > 25 ? speed : 0; drive->prepareDrive(dir, speed, 0); diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index 5a55a98..5ad9e1b 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -45,8 +45,8 @@ blue_led.on() ############################################################################## -thresholds = [ (34, 64, -11, 7, 31, 71), # thresholds yellow goal - (24, 44, -25, 7, -36, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (41, 68, 0, 30, 42, 103), # thresholds yellow goal + (30, 50, -16, 12, -53, -15)] # thresholds blue goal (6, 31, -15, 4, -35, 0) roi = (50,5,250, 230)