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

include/motors_movement/drivecontroller.h

@@ -10,9 +10,9 @@
 
 //BEST NUMBERS YET
 //USE MOVING AVERAGE AND ANGLE WRAP
-#define KP 1.5
+#define KP 0.8
-#define KI 0
+#define KI 0.0
-#define KD 0.1
+#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;
 

include/motors_movement/motor.h

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

include/strategy_roles/keeper.h

@@ -2,23 +2,7 @@
 
 #include "strategy_roles/game.h"
 
-#define KEEPER_ATTACK_DISTANCE 190
+#define KEEPER_N_POINTS 3
-#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
 
 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;
+        int point_spacing, ball_x;
-
-        float angle, angleX, angleY;
-        elapsedMillis t, keeperAttackTimer;
-        bool keeper_tookTimer, keeper_backToGoalPost;
 };

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)

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

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

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) {
+    // Create a smooth transitioning between the old and new speed value, to avoid the motor drivers going into overcurrent protection
-        // no brake ma motore inerte corto a massa e vel=0 contro freno
+    diff = abs(abs(speed) - abs(oldSpeed));
-        // dove corto a VCC e vel=max
+
-        VAL_INA = 0;
+    if(diff < 10) return;
-        VAL_INB = 0;
+
-    } else if (speed > 0) {
+    micros_wait = (float)DRIVE_DURATION_MS/diff * 1000;
-        // clockwise
+    float micros_wait_half = micros_wait * 0.5;
-        VAL_INA = 1;
+    
-        VAL_INB = 0;
+    if(oldSpeed > 0 && speed > 0){
-    } else if (speed < 0) {
+        
-        // counterclockwise
+        digitalWriteFast(pinA, HIGH);
-        VAL_INA = 0;
+        digitalWriteFast(pinB, LOW);
-        VAL_INB = 1;
+
-        speed *= -1;
+        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);
+    else if(oldSpeed < 0 && speed < 0){
-    digitalWrite(pinB, VAL_INB);
+        digitalWriteFast(pinA, LOW);
-    analogWrite(pinPwm, speed);
+        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);
+    digitalWriteFast(pinA, 0);
-    digitalWrite(pinB, 1);
+    digitalWriteFast(pinB, 1);
     analogWrite(pinPwm, 255);
     delay(1500);
 
-    digitalWrite(pinA, 1);
+    digitalWriteFast(pinA, 1);
-    digitalWrite(pinB, 0);
+    digitalWriteFast(pinB, 0);
     analogWrite(pinPwm, 255);
     delay(1500);
 

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{
+    // }else{
-    DEBUG.println("Not seeing ball");
+    // DEBUG.println("Not seeing ball");
-    }
+    // }
 }

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 <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(){
+void Keeper::init()
-    defSpeed = 0;
+{
-    defDir = 0;
+    point_spacing = (abs(CAMERA_GOAL_MIN_X) + abs(CAMERA_GOAL_MAX_X)) / KEEPER_N_POINTS;
-    angle = 0;
-    angleX = 0;
-    angleY = 0;
-    t = 0;
-    keeperAttackTimer = 0;
-    keeper_tookTimer = false;
-    keeper_backToGoalPost = false;
 }
 
-void Keeper::realPlay() {
+void Keeper::realPlay()
-    if(ball->ballSeen) keeper();
+{
-    else ((PositionSysCamera*)ps)->centerGoal();
+    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){
+    // for (int i = CAMERA_GOAL_MIN_X; i <= CAMERA_GOAL_MAX_X; i += point_spacing)
-        // Ball is quite near
+    //     if (ball_x < i)
-        striker->play();
+    //     {
-        if(!this->ls->tookLine){
+            ((PositionSysCamera *)ps)->setMoveSetpoints(ball_x, CAMERA_GOAL_Y);
-            keeperAttackTimer = 0;
+        //     break;
-            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;
-            }
-        }
-    }
-}

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