From 146eb4d6fc772bd44c79d21ff1b4d2968682e0d9 Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Wed, 13 Nov 2019 16:26:03 +0100
Subject: [PATCH] Motors and recenter working

---
 include/drivecontroller.h | 10 ++++++----
 include/motor.h           |  1 +
 include/vars.h            |  8 +++++++-
 src/drivecontroller.cpp   | 30 +++++++++++++-----------------
 src/main.cpp              |  3 ++-
 src/motor.cpp             |  5 +++++
 src/sensors.cpp           |  2 +-
 7 files changed, 35 insertions(+), 24 deletions(-)

diff --git a/include/drivecontroller.h b/include/drivecontroller.h
index a6cafab..0667471 100644
--- a/include/drivecontroller.h
+++ b/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 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();
 
diff --git a/include/motor.h b/include/motor.h
index 1330545..98d0054 100644
--- a/include/motor.h
+++ b/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:
diff --git a/include/vars.h b/include/vars.h
index d02da3c..cfb863d 100644
--- a/include/vars.h
+++ b/include/vars.h
@@ -1,2 +1,8 @@
 #pragma once
-#define DEBUG_PRINT Serial
\ No newline at end of file
+#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
\ No newline at end of file
diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp
index 853ddf9..5e801df 100644
--- a/src/drivecontroller.cpp
+++ b/src/drivecontroller.cpp
@@ -2,15 +2,11 @@
 #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);
         cosins[i] =  (float) cos(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);
diff --git a/src/main.cpp b/src/main.cpp
index ed99cd4..a5afbac 100644
--- a/src/main.cpp
+++ b/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());
 }
\ No newline at end of file
diff --git a/src/motor.cpp b/src/motor.cpp
index 80c1bfe..2c94a36 100644
--- a/src/motor.cpp
+++ b/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) {
diff --git a/src/sensors.cpp b/src/sensors.cpp
index 79779cd..747493d 100644
--- a/src/sensors.cpp
+++ b/src/sensors.cpp
@@ -3,7 +3,7 @@
 
 void initSensors(){
     compass = new DataSourceBNO055();
-    drive = new DriveController(new Motor(12, 11, 2, 45), new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
+    drive = new DriveController(new Motor(12, 11, 2, 45),new  Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
 }
 
 void updateSensors(){