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

motors: ramp up and slow down of motors instead of bang.bang

code_newgen
EmaMaker 2021-02-22 18:43:43 +01:00
parent a4be313e96
commit 004c4a47de
10 changed files with 158 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
include/motors_movement/drivecontroller.h
View File

@ -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;

6
include/motors_movement/motor.h
View File

@ -1,6 +1,9 @@
#pragma once
#include <Arduino.h>
#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;
};

25
include/strategy_roles/keeper.h
View File

@ -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;
};

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

@ -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)

4
src/main.cpp
View File

@ -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();

2
src/motors_movement/drivecontroller.cpp
View File

@ -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);

125
src/motors_movement/motor.cpp
View File

@ -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);
}
digitalWrite(pinA, VAL_INA);
digitalWrite(pinB, VAL_INB);
analogWrite(pinPwm, speed);
} 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);
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);

8
src/sensors/data_source_ball.cpp
View File

@ -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");
// }
}

87
src/strategy_roles/keeper.cpp
View File

@ -6,76 +6,41 @@
#include "strategy_roles/keeper.h"
#include "strategy_roles/games.h"
#include "systems/position/positionsys_camera.h"
#include <Arduino.h>
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);
}
}
}
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;
}
}
}
// 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;
// }
}

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

@ -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
@ -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);