en bordel, début de résultats

include/IMUsensor.hpp

@@ -2,72 +2,38 @@
 #define DEF_IMUSENSOR
 
 #include <Arduino.h>
+#include "datastructs.hpp"
 #include <Wire.h>
 
 
 class IMUsensor
 {
-public:
-  IMUsensor();
-  void resetOrientation() { gyro_angle_set = false ;}
-
-  bool calibrateSensors();
-  bool actualizeSensorData();
-
-  bool calcAbsoluteOrientation( float complementary_rate );
-
-
-  //accesseurs
-
-  float getAngularSpeedX() { return last_loop_gyro_x ; }
-  float getAngularSpeedY() { return last_loop_gyro_y ; }
-  float getAngularSpeedZ() { return last_loop_gyro_z ; }
-
-  float getX() { return orientation_x - 0.5 ; }
-  float getY() { return orientation_y + 1	; }
-  float getZ() { return orientation_z ; }
-
-
-
-
-private:
-  int16_t raw_gyro_x; //raw gyroscope data coming from the IMU
-  int16_t raw_gyro_y;
-  int16_t raw_gyro_z;
-
-  int16_t offset_gyro_x; //offset from the calibration of the IMU
-  int16_t offset_gyro_y;
-  int16_t offset_gyro_z;
-
-  int16_t raw_accel_x; //raw accelerometer data coming from the IMU
-  int16_t raw_accel_y;
-  int16_t raw_accel_z;
-
-  float last_loop_gyro_x; //degrees per second since the last loop
-  float last_loop_gyro_y;
-  float last_loop_gyro_z;
-
-  float accel_x; //absolute orientation from accelerometer
-  float accel_y;
-  float accel_z;
-
-  float gyro_x; //absolute orientation from accelerometer
-  float gyro_y;
-  float gyro_z;
-
-
-
-  float orientation_x; //absolute orientation
-  float orientation_y;
-  float orientation_z;
-
-  int16_t raw_temperature;
-
-  bool gyro_angle_set;
-
-  unsigned long last_data_refresh;
-  float time_loop;
-
+    public:
+        IMUsensor();
+        void resetOrientation() { gyro_angle_set = false ;}
+
+        bool calibrateSensors();
+        bool actualizeSensorData();
+
+        bool calcAbsoluteOrientation( float complementary_rate );
+
+
+        //accesseurs
+        vec3f getRotation() { return last_loop_gyro; };
+        vec3f getOrientation() { return orientation; };
+
+    private:
+        vec3int16 raw_gyro;
+        vec3int16 offset_gyro;
+        vec3int16 raw_accel;
+        vec3f last_loop_gyro;
+        vec3f accel;
+        vec3f gyro;
+        vec3f orientation;
+        int16_t raw_temperature;
+        bool gyro_angle_set;
+        unsigned long last_data_refresh;
+        float time_loop;
 };
 
 #endif

include/datastructs.hpp

@@ -1,5 +1,12 @@
 #ifndef DATASTRUCTS
 #define DATASTRUCTS
+#include <Arduino.h>
+
+struct vec3int16 {
+    int16_t x;
+    int16_t y;
+    int16_t z;
+};
 
 struct gain3f
 {

include/motormanager.hpp

@@ -4,29 +4,33 @@
 #include <Arduino.h>
 #include <Wire.h>
 #include <Servo.h>
+#include "datastructs.hpp"
 
 
 class MotorManager
 {
-public:
-  MotorManager();
+    public:
+        MotorManager();
+
+        void command(float command_x, float command_y, float command_z, float command_h);
+        float getMotorValue(int motor_id);
+        void startMotors();
+        void setOn();
+        void setOff();
+        vec4f getMotorValues() {
+            return {motor_value[0], motor_value[1], motor_value[2], motor_value[3]};
+        }
+    private:
+        float motor_value[4];
+        Servo motor[4];
+
+        float gain_x;
+        float gain_y;
+        float gain_z;
+        float gain_h;
+
+        bool stop_motor;
 
-  void command(float command_x, float command_y, float command_z, float command_h);
-  float getMotorValue(int motor_id);
-  void startMotors();
-  void setOn();
-  void setOff();
-
-private:
-  float motor_value[4];
-  Servo motor[4];
-
-  float gain_x;
-  float gain_y;
-  float gain_z;
-  float gain_h;
-
-  bool stop_motor;
 
 };
 

out.plot

@@ -0,0 +1,14 @@
+
+plot 'data6.csv' using 0:1 with lines title 'X',\
+'data6.csv' using 0:2 with lines title 'Y',\
+'data6.csv' using 0:3 with lines title 'X point',\
+'data6.csv' using 0:4 with lines title 'Y point',\
+'data6.csv' using 0:5 with lines title 'M0',\
+'data6.csv' using 0:7 with lines title 'M2',\
+'data6.csv' using 0:(-3*$2) with lines title 'csigne X point'
+
+
+while (1) {  # make a new 'gpio.dat' every cycle with fresh data
+    replot
+    pause 3
+}

platformio.ini

@@ -6,11 +6,12 @@
 ;   Advanced options: extra scripting
 ;
 ; Please visit documentation for the other options and examples
-; http://docs.platformio.org/page/projectconf.html
+; https://docs.platformio.org/page/projectconf.html
 
 [env:teensy31]
 platform = teensy
 board = teensy31
 framework = arduino
 build_flags = -Iinclude
 upload_protocol = teensy-cli
+lib_deps = teckel12/NewPing@^1.9.7

src/<

@@ -0,0 +1,88 @@
+#include "motormanager.hpp"
+
+MotorManager::MotorManager()
+{
+    for(int i = 0; i <= 3 ; i++)
+    {
+        motor_value[i] = 0;
+    }
+
+    motor[0].attach(6);//23
+    motor[1].attach(7);//22
+    motor[2].attach(9);//20
+    motor[3].attach(8);//21
+
+    gain_x = 1;
+    gain_y = 1;
+    gain_z = 1;
+    gain_h = 1;
+
+    stop_motor = 1;
+
+}
+void MotorManager::setOn()
+{
+    stop_motor = 0;
+}
+
+
+void MotorManager::setOff()
+{
+    for(unsigned int i = 0 ; i < 4 ; i++)
+    {
+        motor_value[i] = 0;
+        motor[i].write(55);
+    }
+
+    stop_motor = 1;
+
+}
+
+void MotorManager::command(float command_x, float command_y, float command_z, float command_h)
+{
+
+    motor_value[0] = gain_y * ( 1 * command_y) + gain_x * (-1 * command_x) + (-1 * command_z) + gain_h * command_h;
+    motor_value[1] = gain_y * ( 1 * command_y) + gain_x * ( 1 * command_x) + ( 1 * command_z) + gain_h * command_h;
+    motor_value[2] = gain_y * (-1 * command_y) + gain_x * ( 1 * command_x) + (-1 * command_z) + gain_h * command_h;
+    motor_value[3] = gain_y * (-1 * command_y) + gain_x * (-1 * command_x) + ( 1 * command_z) + gain_h * command_h;
+
+    for(int i = 0; i < 4 ; i++)
+    {
+        if(! stop_motor)
+        {
+            motor[i].write(writeMicroseconds(1000 + motor_value[i]*1000); // moteurs calibrés pour être à zéro pour 1000 et au max à 2000
+        }
+        else
+        {
+            motor_value[i] = 0;
+            motor[i].write(55);
+        }
+    }
+
+}
+
+void MotorManager::startMotors()    //needed to "arm" the ESC, without that, they wont start.
+{
+    delay(100);
+    for(int i = 0; i <= 3 ; i++)
+    {
+        motor[i].write(10);
+    }
+    delay(100);
+    for(int i = 0; i <= 3 ; i++)
+    {
+        motor[i].write(55);
+    }
+
+    stop_motor = 0;
+}
+
+float MotorManager::getMotorValue(int motor_id)
+{
+    if(motor_id < 4 && motor_id >= 0)
+    {
+        return motor_value[motor_id];
+    }
+
+    return 0;
+}