Added motor support, should work out of the box

include/data_source.h

@@ -1,3 +1,5 @@
+#pragma once
+
 #include "Wire.h"
 #include "Arduino.h"
 #include "HardwareSerial.h"

include/data_source_bno055.h

@@ -1,3 +1,5 @@
+#pragma once
+
 #include "data_source.h"
 #include <Adafruit_BNO055.h>
 #include <Arduino.h>

include/drivecontroller.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <Arduino.h>
+#include "motor.h"
+
+//PID Constants
+#define KP 1.2
+#define KI 0 
+#define KD 0.7  
+
+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 drivePrepared();
+        float updatePid();
+
+    private:
+        Motor* m1;
+        Motor* m2;
+        Motor* m3;
+        Motor* m4;
+        int pDir, pSpeed, pTilt;
+        float speed1, speed2, speed3, speed4, errorePre, integral, pidfactor, errorP, errorD, errorI, delta;
+        int vx, vy;
+        
+        float sins[360], cosins[360];
+
+};

include/motor.h

@@ -1,13 +1,14 @@
+#pragma once
+
 #include <Arduino.h>
 
 class Motor {
 
     public:
-        Motor(int a, int b, byte pwm);
-        void drive(int dir, int speed);
+        Motor(int a, int b, int pwm, int angle);
+        void drive(int speed);
     
     public:
-        int pinA, pinB;
-        byte pinPwm;
+        int pinA, pinB, pinPwm, angle;
 
 };

include/sensors.h

@@ -0,0 +1,16 @@
+#include <Arduino.h>
+#include "data_source_bno055.h"
+#include "motor.h"
+#include "drivecontroller.h"
+
+#ifdef SENSORS_CPP
+#define extr 
+#else
+#define extr extern
+#endif
+
+void initSensors();
+void updateSensors();
+
+extr DataSourceBNO055* compass;
+extr DriveController* drive;

include/vars.h

@@ -1 +1,2 @@
+#pragma once
 #define DEBUG_PRINT Serial

src/drivecontroller.cpp

@@ -0,0 +1,87 @@
+#include "drivecontroller.h"
+#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_;
+
+    for(int i = 0; i < 360; i++){
+        sins[i] = (float) sin(i);
+        cosins[i] =  (float) cos(i);
+    }
+}
+
+void DriveController::prepareDrive(int dir, int speed, int tilt){
+    pDir = dir;
+    pSpeed = speed;
+    pTilt = tilt;
+}
+
+void DriveController::drivePrepared(){
+    drive(pDir, pSpeed, pTilt);
+}
+
+void DriveController::drive(int dir, int speed, int tilt){
+    vx = ((speed * cosins[dir]));
+    vy = ((-speed * sins[dir]));
+
+    speed1 = ((vx * sins[m1->angle] ) + (vy * cosins[m1->angle] ));
+    speed2 = ((vx * sins[m2->angle]) + (vy * cosins[m2->angle]));
+    speed3 = -(speed1);
+    speed4 = -(speed2);
+
+    /*speed1 = ((-(sins[((direzione - 45) + 360) % 360])) * vMot);          // mot 1
+    speed2 = ((-(sins[((direzione - 135) + 360)% 360])) * vMot);          // mot 2
+    speed3 = -(speed1);                                                   // mot 3
+    speed4 = -(speed2);*/ 
+
+    // calcola l'errore di posizione rispetto allo 0
+    delta = compass->getValue();
+    if(delta > 180) delta = delta-360;
+    delta = delta - pTilt;
+
+    // calcola correzione proporzionale
+    errorP = KP * delta;
+
+    // calcola correzione derivativa
+    errorD = KD * (delta - errorePre);
+    errorePre = delta;
+
+    // calcola correzione integrativa
+    integral = 0.5 * integral + delta;
+    errorI = KI * integral;
+    // calcota correzione complessiva
+    pidfactor =  errorD + errorP + errorI;               
+
+    speed1 += pidfactor;
+    speed2 += pidfactor;
+    speed3 += pidfactor;
+    speed4 += pidfactor;
+
+    speed1 = constrain(speed1, -255, 255);
+    speed2 = constrain(speed2, -255, 255);
+    speed3 = constrain(speed3, -255, 255);
+    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, -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;        
+
+    m1->drive((int) speed1);
+    m2->drive((int) speed2);
+    m3->drive((int) speed3);
+    m4->drive((int) speed4);
+} 

src/main.cpp

@@ -1,20 +1,18 @@
 #include <Arduino.h>
 #include "vars.h"
 #include "data_source_bno055.h"
-// #include <imu_class/imu.cpp>
-//cyao c:
+#include "drivecontroller.h"
+#include "sensors.h"
 
-DataSourceBNO055* compass;
 
 void setup() {
   DEBUG_PRINT.begin(9600);
-  compass = new DataSourceBNO055();
+  initSensors();
 }
 
 void loop() {
-  compass->readSensor();
-  DEBUG_PRINT.println(compass->getValue());
+  updateSensors();
 
-  //DEBUG_PRINT.println("Ciaooo");
-  delay(100);
+  //should recenter using predefined values
+  drive->drive();
 }

src/motor.cpp

@@ -1,11 +1,35 @@
 #include "motor.h"
+#include <Arduino.h>
 
-Motor::Motor(int a, int b, byte pwm){
-    pinA = a;
-    pinB = b;
-    pinPwm = pwm;
+Motor::Motor(int a, int b, int pwm, int angle_){
+    this->pinA = a;
+    this->pinB = b;
+    this->pinPwm = pwm;
+    this->angle = angle_;
+
+    pinMode(pinA, OUTPUT);
+    pinMode(pinB, OUTPUT);
+    pinMode(pinPwm, OUTPUT);
 }
 
-void Motor::drive(int dir, int speed){
-    
+void Motor::drive(int speed){
+    byte VAL_INA, VAL_INB;
+    if (speed == 0) {
+        // no brake ma motore inerte corto a massa e vel=0 contro freno
+        // dove corto a VCC e vel=max
+        VAL_INA = 0;
+        VAL_INB = 0;
+    } else if (speed > 0) {
+        // clockwise
+        VAL_INA = 1;
+        VAL_INB = 0;
+    } else if (speed < 0) {
+        // counterclockwise
+        VAL_INA = 0;
+        VAL_INB = 1;
+        speed *= -1;
+    }
+    digitalWrite(pinA, VAL_INA);
+    digitalWrite(pinB, VAL_INB);
+    analogWrite(pinPwm, speed);
 }

src/sensors.cpp

@@ -0,0 +1,11 @@
+#define SENSORS_CPP
+#include "sensors.h"
+
+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));
+}
+
+void updateSensors(){
+    compass->readSensor();
+}