Lines not working well, fixed missing initializations that caused the code to crash

.vscode/settings.json

@@ -20,6 +20,29 @@
         "vector": "cpp",
         "string_view": "cpp",
         "initializer_list": "cpp",
-        "*.tcc": "cpp"
+        "*.tcc": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "cstdarg": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "exception": "cpp",
+        "fstream": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "cinttypes": "cpp",
+        "typeinfo": "cpp"
     }
 }

include/README

@@ -1,39 +0,0 @@
-
-This directory is intended for project header files.
-
-A header file is a file containing C declarations and macro definitions
-to be shared between several project source files. You request the use of a
-header file in your project source file (C, C++, etc) located in `src` folder
-by including it, with the C preprocessing directive `#include'.
-
-```src/main.c
-
-#include "header.h"
-
-int main (void)
-{
- ...
-}
-```
-
-Including a header file produces the same results as copying the header file
-into each source file that needs it. Such copying would be time-consuming
-and error-prone. With a header file, the related declarations appear
-in only one place. If they need to be changed, they can be changed in one
-place, and programs that include the header file will automatically use the
-new version when next recompiled. The header file eliminates the labor of
-finding and changing all the copies as well as the risk that a failure to
-find one copy will result in inconsistencies within a program.
-
-In C, the usual convention is to give header files names that end with `.h'.
-It is most portable to use only letters, digits, dashes, and underscores in
-header file names, and at most one dot.
-
-Read more about using header files in official GCC documentation:
-
-* Include Syntax
-* Include Operation
-* Once-Only Headers
-* Computed Includes
-
-https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

include/data_source_controller.h

@@ -1,22 +0,0 @@
-#include "data_source.h"
-#include "vars.h"
-#include <Arduino.h>
-#include <vector>
-
-using namespace std;
-class DataSourceController {
-
-    public:
-        DataSourceController();
-        DataSourceController(vector<DataSource*>);
-
-    public:
-        void update();
-        void test();
-        void postProcess();
-        void readSensor();
-        void getValue();
-
-        vector<DataSource*> ds;
-
-};

include/drivecontroller.h

@@ -4,10 +4,10 @@
 #include "motor.h"
 
 //PID Constants
-#define KP 0.9
+
-#define KI 0 
+#define KP 1.2    
-#define KD 1.1
+#define KI 0   
-#define DEADZONE_MIN 25
+#define KD 0.7 
 
 class DriveController{
 
@@ -21,6 +21,10 @@ class DriveController{
         float updatePid();
         float torad(float f);
 
+        int vxp, vyp, vxn, vyn;
+        bool canUnlock;
+        elapsedMillis unlockTime;
+
     private:
         Motor* m1;
         Motor* m2;

include/ds_ctrl.h

@@ -0,0 +1,23 @@
+#pragma once
+
+#include "data_source.h"
+#include "vars.h"
+#include <Arduino.h>
+#include <vector>
+
+using namespace std;
+class DataSourceCtrl {
+
+    public:
+        DataSourceCtrl();
+        DataSourceCtrl(vector<DataSource*>);
+
+    public:
+        virtual void update();
+        virtual void test();
+        virtual void postProcess();
+        virtual void read();
+
+        vector<DataSource*> ds;
+
+};

include/ds_ctrl_lines.h

@@ -0,0 +1,40 @@
+#pragma once
+#include "ds_ctrl.h"
+#include "data_source.h"
+#include "vars.h" 
+#include <Arduino.h>
+
+#define S1I A14
+#define S1O A15
+#define S2I A16
+#define S2O A17
+#define S3I A20
+#define S3O A0
+#define S4I A1
+#define S4O A2
+
+#define LINE_THRESH 90
+#define EXTIME 100
+
+class DataSourceCtrlLines : public DataSourceCtrl {
+
+    public:
+        DataSourceCtrlLines();
+        DataSourceCtrlLines(vector<DataSource*> in_, vector<DataSource*> out);
+
+        void read() override;
+        void postProcess() override;
+        void outOfBounds();
+        void handleIntern();
+        void handleExtern();
+        int getValue();
+        void test() override;
+        
+    private:
+        vector<DataSource*> in, out;
+        DataSource* ds;
+        bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow;
+        int inV, outV, inVOldX, inVOldY, value, linetriggerI[4], linetriggerO[4], i;
+        elapsedMillis exitTimer;
+        int outDir, outVel;
+};

include/sensors.h

@@ -3,8 +3,9 @@
 #include "data_source_ball.h"
 #include "data_source_camera.h"
 #include "data_source_us.h"
+#include "ds_ctrl_lines.h"
 #include "motor.h"
-#include "data_source_controller.h"
+#include "ds_ctrl.h"
 #include "drivecontroller.h"
 
 #ifdef SENSORS_CPP
@@ -19,6 +20,10 @@ void updateSensors();
 extr DataSource* compass;
 extr DataSource* ball;
 extr DataSource* camera;
-//extr DataSource* us;
+extr DataSourceCtrl* usCtrl;
-extr DataSourceController* usCtrl;
+extr DataSourceCtrlLines* linesCtrl;
 extr DriveController* drive;
+
+extr vector<DataSource*> lIn;
+extr vector<DataSource*> lOut;
+extr vector<DataSource*> dUs;

include/vars.h

@@ -1,8 +1,10 @@
 #pragma once
-#define DEBUG_PRINT Serial
+#define DEBUG Serial
+
 #define LED_R 20
 #define LED_Y 17
 #define LED_G 13
+
 #define BUZZER 30
 #define SWITCH_SX 28
 #define SWITCH_DX 29