diff --git a/include/data_source.h b/include/data_source.h index 2144df0..33ec4a6 100644 --- a/include/data_source.h +++ b/include/data_source.h @@ -1,3 +1,5 @@ +#pragma once + #include "Wire.h" #include "Arduino.h" #include "HardwareSerial.h" diff --git a/include/data_source_bno055.h b/include/data_source_bno055.h index ad7e9c0..b93fba7 100644 --- a/include/data_source_bno055.h +++ b/include/data_source_bno055.h @@ -1,3 +1,5 @@ +#pragma once + #include "data_source.h" #include #include diff --git a/include/drivecontroller.h b/include/drivecontroller.h new file mode 100644 index 0000000..a6cafab --- /dev/null +++ b/include/drivecontroller.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#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]; + +}; \ No newline at end of file diff --git a/include/motor.h b/include/motor.h index 6ca6c63..1330545 100644 --- a/include/motor.h +++ b/include/motor.h @@ -1,13 +1,14 @@ +#pragma once + #include 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; }; \ No newline at end of file diff --git a/include/sensors.h b/include/sensors.h new file mode 100644 index 0000000..a31ba70 --- /dev/null +++ b/include/sensors.h @@ -0,0 +1,16 @@ +#include +#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; \ No newline at end of file diff --git a/include/vars.h b/include/vars.h index c27f673..d02da3c 100644 --- a/include/vars.h +++ b/include/vars.h @@ -1 +1,2 @@ +#pragma once #define DEBUG_PRINT Serial \ No newline at end of file diff --git a/src/data_source_bno055.cpp b/src/data_source_bno055.cpp index d4e6e2d..4725bd2 100644 --- a/src/data_source_bno055.cpp +++ b/src/data_source_bno055.cpp @@ -15,4 +15,4 @@ void DataSourceBNO055::readSensor(){ imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER); value = (int) euler.x(); // } -} \ No newline at end of file +} diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp new file mode 100644 index 0000000..111ac35 --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index a1927ce..ed99cd4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,20 +1,18 @@ #include #include "vars.h" #include "data_source_bno055.h" -// #include -//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(); } \ No newline at end of file diff --git a/src/motor.cpp b/src/motor.cpp index 92c7fa4..80c1bfe 100644 --- a/src/motor.cpp +++ b/src/motor.cpp @@ -1,11 +1,35 @@ #include "motor.h" +#include -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); } \ No newline at end of file diff --git a/src/sensors.cpp b/src/sensors.cpp new file mode 100644 index 0000000..79779cd --- /dev/null +++ b/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(); +} \ No newline at end of file