diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index 64fddc9..eff642d 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -10,9 +10,9 @@ //BEST NUMBERS YET //USE MOVING AVERAGE AND ANGLE WRAP -#define KP 1.5 -#define KI 0 -#define KD 0.1 +#define KP 0.8 +#define KI 0.0 +#define KD 0.025 #define KSPD 0.3 @@ -41,6 +41,7 @@ class DriveController{ private: PID* pid; ComplementaryFilter* speedFilter; + ComplementaryFilter* dirFilter; int pDir, pSpeed, pTilt, oldSpeed; float x, y, vx, vy, speed1, speed2, speed3, speed4, pidfactor, delta; diff --git a/include/motors_movement/motor.h b/include/motors_movement/motor.h index acca9aa..4c8dc7b 100644 --- a/include/motors_movement/motor.h +++ b/include/motors_movement/motor.h @@ -1,6 +1,9 @@ #pragma once #include +#include "behaviour_control/complementary_filter.h" + +#define DRIVE_DURATION_MS 5 class Motor { @@ -11,6 +14,7 @@ class Motor { void test(); public: - int pinA, pinB, pinPwm, angle; + int pinA, pinB, pinPwm, angle, oldSpeed, diff; + float micros_wait; }; \ No newline at end of file diff --git a/include/strategy_roles/keeper.h b/include/strategy_roles/keeper.h index c207510..1245dfd 100644 --- a/include/strategy_roles/keeper.h +++ b/include/strategy_roles/keeper.h @@ -2,23 +2,7 @@ #include "strategy_roles/game.h" -#define KEEPER_ATTACK_DISTANCE 190 -#define KEEPER_ALONE_ATTACK_TIME 5000 //in millis -#define KEEPER_COMRADE_ATTACK_TIME 3000//in millis -#define KEEPER_BASE_VEL 320 -#define KEEPER_VEL_MULT 1.4 -#define KEEPER_BALL_BACK_ANGLE 30 -#define KEEPER_ANGLE_SX 265 -#define KEEPER_ANGLE_DX 85 - -//POSITION -#define CENTERGOALPOST_VEL1 220 -#define CENTERGOALPOST_VEL2 220 -#define CENTERGOALPOST_VEL3 220 -#define CENTERGOALPOST_CAM_MIN -40 -#define CENTERGOALPOST_CAM_MAX 40 -#define CENTERGOALPOST_US_MAX 60 -#define CENTERGOALPOST_US_CRITIC 25 +#define KEEPER_N_POINTS 3 class Keeper : public Game{ @@ -30,11 +14,6 @@ class Keeper : public Game{ void realPlay() override; void init() override; void keeper(); - void centerGoalPostCamera(bool); - int defSpeed, defDir; - - float angle, angleX, angleY; - elapsedMillis t, keeperAttackTimer; - bool keeper_tookTimer, keeper_backToGoalPost; + int point_spacing, ball_x; }; diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 80de44f..a0fa4df 100644 --- a/include/systems/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -9,10 +9,13 @@ #define CAMERA_CENTER_Y 20 //Camera goal: those setpoints correspond to the position of the center of the goal on the field -#define CAMERA_GOAL_X 0 +#define CAMERA_GOAL_X 10 #define CAMERA_GOAL_Y 0 -#define CAMERA_CENTER_Y_ABS_SUM 50 +#define CAMERA_GOAL_MIN_X -15 +#define CAMERA_GOAL_MAX_X 35 + +#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) diff --git a/src/main.cpp b/src/main.cpp index e326670..bfab336 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -39,7 +39,9 @@ void loop() { // striker_test->play(1); striker->play(1); - // keeper->play(role==0); + // keeper->play(1); + + // drive->prepareDrive(0, 100,0); // 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 e1a25bb..21f12b9 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -28,7 +28,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_) pid = new PID(&input, &output, &setpoint, KP, KI, KD, 1,DIRECT); - pid->SetSampleTime(1.5); + pid->SetSampleTime(2.5); pid->setAngleWrap(true); pid->SetDerivativeLag(2); pid->SetOutputLimits(-255,255); diff --git a/src/motors_movement/motor.cpp b/src/motors_movement/motor.cpp index da8596e..4765329 100644 --- a/src/motors_movement/motor.cpp +++ b/src/motors_movement/motor.cpp @@ -7,45 +7,124 @@ Motor::Motor(int a, int b, int pwm, int angle_){ this->pinB = b; this->pinPwm = pwm; this->angle = angle_; + this->oldSpeed = 0; + this->diff = 0; + this->micros_wait = 0; pinMode(pinA, OUTPUT); pinMode(pinB, OUTPUT); pinMode(pinPwm, OUTPUT); - analogWriteFrequency(pinPwm, 5000); + analogWriteFrequency(pinPwm, 15000); } Motor::Motor(){ } void Motor::drive(int speed){ - byte VAL_INA, VAL_INB; - if (speed == 0) { - // no brake ma motore inerte corto a massa e vel=0 contro freno - // dove corto a VCC e vel=max - VAL_INA = 0; - VAL_INB = 0; - } else if (speed > 0) { - // clockwise - VAL_INA = 1; - VAL_INB = 0; - } else if (speed < 0) { - // counterclockwise - VAL_INA = 0; - VAL_INB = 1; - speed *= -1; + + // Create a smooth transitioning between the old and new speed value, to avoid the motor drivers going into overcurrent protection + diff = abs(abs(speed) - abs(oldSpeed)); + + if(diff < 10) return; + + micros_wait = (float)DRIVE_DURATION_MS/diff * 1000; + float micros_wait_half = micros_wait * 0.5; + + if(oldSpeed > 0 && speed > 0){ + + digitalWriteFast(pinA, HIGH); + digitalWriteFast(pinB, LOW); + + if(oldSpeed < speed){ + // Ramp up + for(int i = oldSpeed; i < speed; i++){ + analogWrite(pinPwm, i); + } + + } else if(oldSpeed > speed) { + // Slow down + for(int i = oldSpeed; i > speed; i--){ + analogWrite(pinPwm, i); + } + } + } - digitalWrite(pinA, VAL_INA); - digitalWrite(pinB, VAL_INB); - analogWrite(pinPwm, speed); + else if(oldSpeed < 0 && speed < 0){ + digitalWriteFast(pinA, LOW); + digitalWriteFast(pinB, HIGH); + + if(oldSpeed < speed){ + // Ramp up + for(int i = oldSpeed; i < speed; i++){ + analogWrite(pinPwm, -i); + } + + } else if(oldSpeed > speed) { + // Slow down + for(int i = oldSpeed; i > speed; i--){ + analogWrite(pinPwm, -i); + } + + } + + } + else if(oldSpeed < 0 && speed > 0){ + + digitalWriteFast(pinA, LOW); + digitalWriteFast(pinB, HIGH); + + for(int i = oldSpeed; i <= 0; i++){ + analogWrite(pinPwm, -i); + } + + digitalWriteFast(pinA, LOW); + digitalWriteFast(pinB, LOW); + + delayMicroseconds(micros_wait); + + digitalWriteFast(pinA, HIGH); + digitalWriteFast(pinB, LOW); + + for(int i = 0; i < speed; i++){ + analogWrite(pinPwm, i); + } + + }else if(oldSpeed > 0 && speed < 0){ + + digitalWriteFast(pinA, HIGH); + digitalWriteFast(pinB, LOW); + + for(int i = oldSpeed; i >= 0; i--){ + analogWrite(pinPwm, i); + delayMicroseconds(micros_wait_half); + } + + + digitalWriteFast(pinA, LOW); + digitalWriteFast(pinB, LOW); + + delayMicroseconds(micros_wait); + + digitalWriteFast(pinA, LOW); + digitalWriteFast(pinB, HIGH); + + for(int i = 0; i > speed; i--){ + analogWrite(pinPwm, -i); + delayMicroseconds(micros_wait_half); + } + + } + + oldSpeed = speed; } void Motor::test(){ - digitalWrite(pinA, 0); - digitalWrite(pinB, 1); + digitalWriteFast(pinA, 0); + digitalWriteFast(pinB, 1); analogWrite(pinPwm, 255); delay(1500); - digitalWrite(pinA, 1); - digitalWrite(pinB, 0); + digitalWriteFast(pinA, 1); + digitalWriteFast(pinB, 0); analogWrite(pinPwm, 255); delay(1500); diff --git a/src/sensors/data_source_ball.cpp b/src/sensors/data_source_ball.cpp index f315fb6..e8134a6 100644 --- a/src/sensors/data_source_ball.cpp +++ b/src/sensors/data_source_ball.cpp @@ -23,13 +23,13 @@ void DataSourceBall :: postProcess(){ void DataSourceBall :: test(){ this->update(); - if(ballSeen){ + // if(ballSeen){ DEBUG.print(angle); DEBUG.print(" | "); DEBUG.print(distance); DEBUG.print(" | "); DEBUG.println(ballSeen); - }else{ - DEBUG.println("Not seeing ball"); - } + // }else{ + // DEBUG.println("Not seeing ball"); + // } } \ No newline at end of file diff --git a/src/strategy_roles/keeper.cpp b/src/strategy_roles/keeper.cpp index 120ef0f..d96e173 100644 --- a/src/strategy_roles/keeper.cpp +++ b/src/strategy_roles/keeper.cpp @@ -6,76 +6,41 @@ #include "strategy_roles/keeper.h" #include "strategy_roles/games.h" #include "systems/position/positionsys_camera.h" +#include - -Keeper::Keeper() : Game() { +Keeper::Keeper() : Game() +{ init(); } -Keeper::Keeper(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_){ +Keeper::Keeper(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) +{ } -void Keeper::init(){ - defSpeed = 0; - defDir = 0; - angle = 0; - angleX = 0; - angleY = 0; - t = 0; - keeperAttackTimer = 0; - keeper_tookTimer = false; - keeper_backToGoalPost = false; +void Keeper::init() +{ + point_spacing = (abs(CAMERA_GOAL_MIN_X) + abs(CAMERA_GOAL_MAX_X)) / KEEPER_N_POINTS; } -void Keeper::realPlay() { - if(ball->ballSeen) keeper(); - else ((PositionSysCamera*)ps)->centerGoal(); +void Keeper::realPlay() +{ + if (ball->ballSeen) + keeper(); + else + ps->centerGoal(); } -void Keeper::keeper() { +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); - if(ball->distance > KEEPER_ATTACK_DISTANCE){ - // Ball is quite near - striker->play(); - if(!this->ls->tookLine){ - keeperAttackTimer = 0; - keeper_tookTimer = true; - } - if(keeperAttackTimer > KEEPER_ALONE_ATTACK_TIME && keeper_tookTimer) keeper_backToGoalPost = true; - - }else{ - - angle = (KEEPER_ANGLE_DX + ball->angle) * M_PI / 180; - angleX = abs(cos(angle)); - - if(ball->angle >= 0 && ball->angle <= KEEPER_ANGLE_DX && CURRENT_DATA_READ.angleDefFix < 30) drive->prepareDrive(KEEPER_ANGLE_DX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT); - else if(ball->angle >= KEEPER_ANGLE_SX && ball->angle <= 360 && CURRENT_DATA_READ.angleDefFix > -30) drive->prepareDrive(KEEPER_ANGLE_SX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT); - else if(ball->angle < KEEPER_ANGLE_SX && ball->angle > KEEPER_ANGLE_DX){ - int ball_degrees2 = ball->angle > 180? ball->angle-360:ball->angle; - int dir = ball_degrees2 > 0 ? ball->angle + KEEPER_BALL_BACK_ANGLE : ball->angle - KEEPER_BALL_BACK_ANGLE; - dir = dir < 0? dir + 360: dir; - - drive->prepareDrive(dir, KEEPER_BASE_VEL); - } - } + // 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; + // } } - -void Keeper::centerGoalPostCamera(bool checkBack){ - if (CURRENT_DATA_READ.angleDefFix > CENTERGOALPOST_CAM_MAX) { - drive->prepareDrive(KEEPER_ANGLE_SX, CENTERGOALPOST_VEL1); - } else if (CURRENT_DATA_READ.angleDefFix < CENTERGOALPOST_CAM_MIN) { - drive->prepareDrive(KEEPER_ANGLE_DX, CENTERGOALPOST_VEL1); - }else if(CURRENT_DATA_READ.angleDefFix > CENTERGOALPOST_CAM_MIN && CURRENT_DATA_READ.angleDefFix < CENTERGOALPOST_CAM_MAX){ - if(!ball->ballSeen) drive->prepareDrive(0, 0, 0); - if(checkBack){ - if(usCtrl->getValue(2) > CENTERGOALPOST_US_MAX){ - drive->prepareDrive(180, CENTERGOALPOST_VEL2); - } else{ - if(usCtrl->getValue(2) < CENTERGOALPOST_US_CRITIC) drive->prepareDrive(0, CENTERGOALPOST_VEL3); - - keeper_backToGoalPost = false; - keeper_tookTimer = false; - } - } - } -} \ No newline at end of file diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index beab2de..b2d90d3 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -19,12 +19,12 @@ PositionSysCamera::PositionSysCamera() { givenMovement = false; X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE); - X->SetOutputLimits(-50,50); + X->SetOutputLimits(-MAX_X, MAX_X); X->SetMode(AUTOMATIC); X->SetDerivativeLag(1); X->SetSampleTime(2); Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE); - Y->SetOutputLimits(-50,50); + Y->SetOutputLimits(-MAX_Y,MAX_Y); Y->SetMode(AUTOMATIC); Y->SetDerivativeLag(1); Y->SetSampleTime(2); @@ -70,6 +70,7 @@ void PositionSysCamera::setMoveSetpoints(int x, int y){ Setpointx = x; Setpointy = y; givenMovement = true; + CameraPID(); } void PositionSysCamera::addMoveOnAxis(int x, int y){ @@ -85,6 +86,7 @@ void PositionSysCamera::goCenter(){ 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 @@ -108,7 +110,7 @@ void PositionSysCamera::CameraPID(){ Setpointy += axisy; X->Compute(); - Y->Compute(); + Y->Compute(); //Compute an X and Y to give to the PID later //There's surely a better way to do this @@ -116,7 +118,7 @@ void PositionSysCamera::CameraPID(){ dir = (dir+360) % 360; int dist = sqrt(Outputx*Outputx + Outputy*Outputy); - int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 120); + int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 80); drive->prepareDrive(dir, speed, 0);