position system camera & motors: proper vector sum, value tweaking

include/motors_movement/drivecontroller.h

@@ -18,11 +18,13 @@
 
 #define UNLOCK_THRESH 800
 
+//Max possible vel 310
+
 #define MAX_VEL 150
-#define MAX_VEL_EIGTH 120
-#define MAX_VEL_HALF 75
-#define MAX_VEL_3QUARTERS 112
-#define MAX_VEL_QUARTER 38
+#define MAX_VEL_EIGTH ((int)MAX_VEL*0.8)
+#define MAX_VEL_HALF ((int)MAX_VEL*0.5)
+#define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75)
+#define MAX_VEL_QUARTER ((int)MAX_VEL*0.25)
 
 class DriveController{
 
@@ -35,6 +37,7 @@ class DriveController{
         void drivePrepared();
         float updatePid();
         float torad(float f);
+        void resetDrive();
 
         int vxp, vyp, vxn, vyn;
         bool canUnlock;

include/systems/lines/linesys_camera.h

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

include/systems/position/positionsys_camera.h

@@ -2,23 +2,23 @@
 
 #include "PID_v2.h"
 #include "systems/systems.h"
+#include "behaviour_control/complementary_filter.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 3
-
+#define CAMERA_TRANSLATION_X 8
+#define CAMERA_TRANSLATION_Y 10
 //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 0
-#define  CAMERA_GOAL_Y -12
+#define  CAMERA_GOAL_Y -14
 
-#define CAMERA_GOAL_MIN_X -14
-#define CAMERA_GOAL_MAX_X 14
+#define CAMERA_GOAL_MIN_X -16
+#define CAMERA_GOAL_MAX_X 16
 
 #define CAMERA_CENTER_Y_ABS_SUM 60
 //Actually it's ± MAX_VAL
@@ -52,5 +52,7 @@ class PositionSysCamera : public PositionSystem{
         bool givenMovement;
         PID* X;
         PID* Y;
+        ComplementaryFilter* filterDir;
+        ComplementaryFilter* filterSpeed;
 
 };

src/main.cpp

@@ -15,7 +15,7 @@ bool keeper_condition = false;
 
 void setup() {
   delay(1500);
-  DEBUG.begin(9600);
+  DEBUG.begin(115200);
     
   for(int i = 0; i < 360; i++){
       sins[i] = (float) sin((i*3.14/180));
@@ -38,7 +38,8 @@ void setup() {
 
 void loop() {
   updateSensors();
-  if(DEBUG.available()) testmenu->testMenu();
+
+  drive->resetDrive();
   
   striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
   keeper_condition = role == LOW;
@@ -46,6 +47,8 @@ void loop() {
   striker->play(striker_condition);
   keeper->play(keeper_condition);
 
+  testmenu->testMenu();
+
   // Last thing to do: movement and update status vector
   drive->drivePrepared();  
   updateStatusVector();

src/motors_movement/drivecontroller.cpp

@@ -68,11 +68,11 @@ void DriveController::drive(int dir, int speed, int tilt){
     //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?
     // Disable vector sum because calculations are a bitty crappy imho. Will have to test if it's what makes the robot act strange with lines
     // 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])) + CURRENT_DATA_READ.addvx;
+    vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
 
-    vx = ((speed * cosins[dir]));
-    vy = ((-speed * sins[dir]));
+    // 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;
@@ -151,4 +151,10 @@ void DriveController::drive(int dir, int speed, int tilt){
     CURRENT_DATA_WRITE.axisBlock[1] = vxn;
     CURRENT_DATA_WRITE.axisBlock[2] = vyp;
     CURRENT_DATA_WRITE.axisBlock[3] = vyn;
-} 
+} 
+
+void DriveController::resetDrive(){
+    CURRENT_DATA_WRITE.addvx = 0;
+    CURRENT_DATA_WRITE.addvy = 0;
+    prepareDrive(0,0,0);
+}

src/motors_movement/motor.cpp

@@ -41,9 +41,20 @@ void Motor::test(){
     analogWrite(pinPwm, 255);
     delay(1500);
 
+
+    digitalWriteFast(pinA, 0);
+    digitalWriteFast(pinB, 0);
+    analogWrite(pinPwm, 0);
+    delay(500);
+
     digitalWriteFast(pinA, 1);
     digitalWriteFast(pinB, 0);
     analogWrite(pinPwm, 255);
     delay(1500);
 
+    digitalWriteFast(pinA, 0);
+    digitalWriteFast(pinB, 0);
+    analogWrite(pinPwm, 0);
+    delay(500);
+
 }

src/system/positions/positionsys_camera.cpp

@@ -28,6 +28,9 @@ PositionSysCamera::PositionSysCamera() {
     Y->SetMode(AUTOMATIC);
     Y->SetDerivativeLag(1);
     Y->SetSampleTime(2);
+
+    filterDir = new ComplementaryFilter(0.65);
+    filterSpeed = new ComplementaryFilter(0.65);
 }
 
 void PositionSysCamera::update(){
@@ -118,19 +121,21 @@ 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);
+        speed = filterSpeed->calculate(speed);
+        speed = speed > 35 ? speed : 0;
+        dir = filterDir->calculate(dir);
+        // drive->prepareDrive(dir, speed, 0);
 
 
         //Disable below lines for now because they probably result in unexpected behaviour on lines. Re enabling them requires to comment out the drive->prepareDrive above
         //and check the notes in drivecontroller for the other stuff to comment and uncomment
 
         //TODO: add complementary filter on this speed if we keep using it
-        // vx = ((speed * cosins[dir]));
-        // vy = ((-speed * sins[dir]));
+        vx = ((speed * cosins[dir]));
+        vy = ((-speed * sins[dir]));
 
-        // CURRENT_DATA_WRITE.addvx = vx;
-        // CURRENT_DATA_WRITE.addvy = vy;
+        CURRENT_DATA_WRITE.addvx = vx;
+        CURRENT_DATA_WRITE.addvy = vy;
     }
 }