Motors and recenter working

include/drivecontroller.h

@@ -4,17 +4,19 @@
 #include "motor.h"
 
 //PID Constants
-#define KP 1.2
+#define KP 0.7
 #define KI 0 
 #define KD 0.7
+#define DEADZONE_MIN 25
 
 class DriveController{
 
     public:
 
         DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_);
-        void drive(int dir = 0, int speed = 0, int tilt = 0);
-        void prepareDrive(int dir = 0, int speed = 0, int tilt = 0);
+        
+        void drive(int dir, int speed, int tilt);
+        void prepareDrive(int dir, int speed, int tilt);
         void drivePrepared();
         float updatePid();
 

include/motor.h

@@ -6,6 +6,7 @@ class Motor {
 
     public:
         Motor(int a, int b, int pwm, int angle);
+        Motor();
         void drive(int speed);
     
     public:

include/vars.h

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

src/drivecontroller.cpp

@@ -2,14 +2,10 @@
 #include "sensors.h"
 
 DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_){
-    // m1 = new Motor(12, 11, 2, 45);
-    // m2 = new Motor(25, 24, 5, 135);
-    // m3 = new Motor(27, 26, 6, 225);
-    // m4 = new Motor(21, 22, 23, 315);
-    this->m1 = m1_;
-    this->m2 = m2_;
-    this->m3 = m3_;
-    this->m4 = m4_;
+    m1 = m1_;
+    m2 = m2_;
+    m3 = m3_;
+    m4 = m4_;
     
     for(int i = 0; i < 360; i++){
         sins[i] = (float) sin(i);
@@ -65,15 +61,15 @@ void DriveController::drive(int dir, int speed, int tilt){
     speed4 = constrain(speed4, -255, 255);
 
     //-almost- eliminating motor deadzones, should increase motor efficiency
-    speed1 = (int)speed1 > 0 ? map((int)speed1, 1, 255, 35, 255) : speed1;        
-    speed2 = (int)speed2 > 0 ? map((int)speed2, 1, 255, 35, 255) : speed2;        
-    speed3 = (int)speed3 > 0 ? map((int)speed3, 1, 255, 35, 255) : speed3;        
-    speed4 = (int)speed4 > 0 ? map((int)speed4, 1, 255, 35, 255) : speed4;        
+    speed1 = (int)speed1 > 0 ? map((int)speed1, 1, 255, DEADZONE_MIN, 255) : speed1;        
+    speed2 = (int)speed2 > 0 ? map((int)speed2, 1, 255, DEADZONE_MIN, 255) : speed2;        
+    speed3 = (int)speed3 > 0 ? map((int)speed3, 1, 255, DEADZONE_MIN, 255) : speed3;        
+    speed4 = (int)speed4 > 0 ? map((int)speed4, 1, 255, DEADZONE_MIN, 255) : speed4;        
 
-    speed1 = (int)speed1 < 0 ? map((int)speed1, -255, -1, -255, -35) : speed1;        
-    speed2 = (int)speed2 < 0 ? map((int)speed2, -255, -1, -255, -35) : speed2;        
-    speed3 = (int)speed3 < 0 ? map((int)speed3, -255, -1, -255, -35) : speed3;        
-    speed4 = (int)speed4 < 0 ? map((int)speed4, -255, -1, -255, -35) : speed4;        
+    speed1 = (int)speed1 < 0 ? map((int)speed1, -255, -1, -255, -DEADZONE_MIN) : speed1;        
+    speed2 = (int)speed2 < 0 ? map((int)speed2, -255, -1, -255, -DEADZONE_MIN) : speed2;        
+    speed3 = (int)speed3 < 0 ? map((int)speed3, -255, -1, -255, -DEADZONE_MIN) : speed3;        
+    speed4 = (int)speed4 < 0 ? map((int)speed4, -255, -1, -255, -DEADZONE_MIN) : speed4;        
 
     m1->drive((int) speed1);
     m2->drive((int) speed2);

src/main.cpp

@@ -14,5 +14,6 @@ void loop() {
   updateSensors();
 
   //should recenter using predefined values
-  drive->drive();
+  drive->drive(0, 0, 0);
+  DEBUG_PRINT.println(compass->getValue());
 }

src/motor.cpp

@@ -1,4 +1,5 @@
 #include "motor.h"
+#include "vars.h"
 #include <Arduino.h>
 
 Motor::Motor(int a, int b, int pwm, int angle_){
@@ -12,6 +13,10 @@ Motor::Motor(int a, int b, int pwm, int angle_){
     pinMode(pinPwm, OUTPUT);
 }
 
+Motor::Motor(){
+    
+}
+
 void Motor::drive(int speed){
     byte VAL_INA, VAL_INB;
     if (speed == 0) {