EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

movement: enable vector sum for multiple movements in a single cycle

code_newgen
EmaMaker 2021-03-22 14:03:53 +01:00
parent 0aeb92ac27
commit f26cbf4984
9 changed files with 98 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
include/motors_movement/drivecontroller.h
View File

@ -18,11 +18,11 @@
#define UNLOCK_THRESH 800 #define UNLOCK_THRESH 800
#define MAX_VEL 310 #define MAX_VEL 150
#define MAX_VEL_EIGTH 248 #define MAX_VEL_EIGTH 120
#define MAX_VEL_HALF 155 #define MAX_VEL_HALF 75
#define MAX_VEL_3QUARTERS 232 #define MAX_VEL_3QUARTERS 112
#define MAX_VEL_QUARTER 78 #define MAX_VEL_QUARTER 38
class DriveController{ class DriveController{

16
include/strategy_roles/keeper.h
View File

@ -1,8 +1,19 @@
#pragma once #pragma once
#include "strategy_roles/game.h" #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{ class Keeper : public Game{
@ -10,6 +21,9 @@ class Keeper : public Game{
Keeper(); Keeper();
Keeper(LineSystem*, PositionSystem*); Keeper(LineSystem*, PositionSystem*);
public:
bool shouldStrike;
private: private:
void realPlay() override; void realPlay() override;
void init() override; void init() override;

2
include/systems/lines/linesys_camera.h
View File

@ -16,7 +16,7 @@
#define S4I A9 #define S4I A9
#define S4O A8 #define S4O A8
#define LINE_THRESH_CAM 600 #define LINE_THRESH_CAM 300
#define EXIT_TIME 250 #define EXIT_TIME 250
#define LINES_EXIT_SPD 350 #define LINES_EXIT_SPD 350

15
include/systems/position/positionsys_camera.h
View File

@ -1,29 +1,30 @@
#include "PID_v2.h" #pragma once
#include "PID_v2.h"
#include "systems/systems.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 /*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. 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*/ These values need to be subtracted from the coords used in setMoveSetpoints*/
#define CAMERA_TRANSLATION_X 0 #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 //Camera center: those setpoints correspond to what we consider the center of the field
#define CAMERA_CENTER_X 0 #define CAMERA_CENTER_X 0
#define CAMERA_CENTER_Y 0 #define CAMERA_CENTER_Y 0
//Camera goal: those setpoints correspond to the position of the center of the goal on the field //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_X 0
#define CAMERA_GOAL_Y 0 #define CAMERA_GOAL_Y -12
#define CAMERA_GOAL_MIN_X -15 #define CAMERA_GOAL_MIN_X -14
#define CAMERA_GOAL_MAX_X 35 #define CAMERA_GOAL_MAX_X 14
#define CAMERA_CENTER_Y_ABS_SUM 60 #define CAMERA_CENTER_Y_ABS_SUM 60
//Actually it's ± MAX_VAL //Actually it's ± MAX_VAL
#define MAX_X 50 #define MAX_X 50
#define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2) #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
#define DIST_MULT 1.65 #define DIST_MULT 1.4
#define Kpx 1 #define Kpx 1
#define Kix 0 #define Kix 0

11
src/main.cpp
View File

@ -10,6 +10,9 @@
TestMenu* testmenu; TestMenu* testmenu;
bool striker_condition = false;
bool keeper_condition = false;
void setup() { void setup() {
delay(1500); delay(1500);
DEBUG.begin(9600); DEBUG.begin(9600);
@ -37,11 +40,11 @@ void loop() {
updateSensors(); updateSensors();
if(DEBUG.available()) testmenu->testMenu(); 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->play(striker_condition);
// striker_test->play(1); keeper->play(keeper_condition);
// keeper->play(role==0);
// Last thing to do: movement and update status vector // Last thing to do: movement and update status vector
drive->drivePrepared(); drive->drivePrepared();

45
src/motors_movement/drivecontroller.cpp
View File

@ -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 // 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; // vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
// vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy; // vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
vx = ((speed * cosins[dir])); vx = ((speed * cosins[dir]));
vy = ((-speed * sins[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)) { // 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; // vxn = 0;
vxp = 0; // vxp = 0;
vyp = 0; // vyp = 0;
vyn = 0; // vyn = 0;
} // }
if((vy > 0 && vxn == 1) || (vy < 0 && vxp == 1)) vy = 0; // if((vy > 0 && vxn == 1) || (vy < 0 && vxp == 1)) vy = 0;
if((vx > 0 && vyp == 1) || (vx < 0 && vyn == 1)) vx = 0; // if((vx > 0 && vyp == 1) || (vx < 0 && vyn == 1)) vx = 0;
speed1 = ((vx * sins[m1->angle] ) + (vy * cosins[m1->angle] )); speed1 = ((vx * sins[m1->angle] ) + (vy * cosins[m1->angle] ));
speed2 = ((vx * sins[m2->angle]) + (vy * cosins[m2->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; speed3 += pidfactor;
speed4 += 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); speed1 = constrain(speed1, -255, 255);
speed2 = constrain(speed2, -255, 255); speed2 = constrain(speed2, -255, 255);
speed3 = constrain(speed3, -255, 255); speed3 = constrain(speed3, -255, 255);

32
src/strategy_roles/keeper.cpp
View File

@ -7,6 +7,8 @@
#include "systems/position/positionsys_camera.h" #include "systems/position/positionsys_camera.h"
#include <Arduino.h> #include <Arduino.h>
int currentPosition = 0;
Keeper::Keeper() : Game() Keeper::Keeper() : Game()
{ {
init(); init();
@ -18,7 +20,7 @@ Keeper::Keeper(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
void Keeper::init() void Keeper::init()
{ {
point_spacing = (abs(CAMERA_GOAL_MIN_X) + abs(CAMERA_GOAL_MAX_X)) / KEEPER_N_POINTS; shouldStrike = false;
} }
void Keeper::realPlay() void Keeper::realPlay()
@ -31,15 +33,23 @@ void Keeper::realPlay()
void Keeper::keeper() void Keeper::keeper()
{ {
//Convert Ball position into a coordinate in the Camera Position Sys plane shouldStrike = false;
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);
// for (int i = CAMERA_GOAL_MIN_X; i <= CAMERA_GOAL_MAX_X; i += point_spacing) if(CURRENT_DATA_READ.ballDistance < KEEPER_ATTACK_DISTANCE || (CURRENT_DATA_READ.ballAngle >= 90 && CURRENT_DATA_READ.ballAngle <= 270)){
// if (ball_x < i) shouldStrike = true;
// { return;
((PositionSysCamera *)ps)->setMoveSetpoints(ball_x, CAMERA_GOAL_Y); }
// break;
// } 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);
} }

4
src/system/positions/positionsys_camera.cpp
View File

@ -77,16 +77,15 @@ void PositionSysCamera::addMoveOnAxis(int x, int y){
axisx += x; axisx += x;
axisy += y; axisy += y;
givenMovement = true; givenMovement = true;
CameraPID();
} }
void PositionSysCamera::goCenter(){ void PositionSysCamera::goCenter(){
setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y); setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y);
CameraPID();
} }
void PositionSysCamera::centerGoal(){ void PositionSysCamera::centerGoal(){
setMoveSetpoints(CAMERA_GOAL_X, CAMERA_GOAL_Y); 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 /*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 dist = sqrt(Outputx*Outputx + Outputy*Outputy);
int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL); int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL);
speed = speed > 25 ? speed : 0;
drive->prepareDrive(dir, speed, 0); drive->prepareDrive(dir, speed, 0);

4
utility/OpenMV/conic_eff_h7.py
View File

@ -45,8 +45,8 @@ blue_led.on()
############################################################################## ##############################################################################
thresholds = [ (34, 64, -11, 7, 31, 71), # thresholds yellow goal thresholds = [ (41, 68, 0, 30, 42, 103), # thresholds yellow goal
(24, 44, -25, 7, -36, -10)] # thresholds blue goal (6, 31, -15, 4, -35, 0) (30, 50, -16, 12, -53, -15)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (50,5,250, 230) roi = (50,5,250, 230)