<commit_before>#include "config.h"
#include <Math.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

//mise en place des variables

Servo motor_right;
Servo motor_left;
Servo motor_front;
Servo motor_back;


// PID variables
bool auto_stabilisation_mode = false; //activer ou pas le mode auto-stabilisé: utiliser le switch de input 5 ou 6
int dt = 3;//notre dt est de 100 millisecondes
double pid_roll_speed_in,   pid_roll_out,   pid_roll_setpoint,  roll_error,  Integral_roll_error,  Derivative_roll_error,  last_roll_error, AS_roll_error, AS_Integral_roll_error, AS_Derivative_roll_error, AS_last_roll_error = 0;
double pid_pitch_speed_in,  pid_pitch_out,  pid_pitch_setpoint, pitch_error, Integral_pitch_error, Derivative_pitch_error, last_pitch_error, AS_pitch_error, AS_Integral_pitch_error, AS_Derivative_pitch_error, AS_last_pitch_error = 0;
double pid_yaw_speed_in,    pid_yaw_out,    pid_yaw_setpoint,   yaw_error,   Integral_yaw_error,   Derivative_yaw_error,   last_yaw_error = 0;

// MOTORS
int mR, mL, mF, mB;

//RX
int throttle = THROTTLE_RMIN;
volatile int input[4];
unsigned long timer[4];
byte last_channel[4];

//IMU variables
float roll_speed, roll_angle;
float pitch_speed, pitch_angle;
float yaw_speed, yaw_angle;
Adafruit_BNO055 bno = Adafruit_BNO055();


////fonctions de lecture du gyro et accel

//Initiation du IMU
void bno_initialisation() {
  bno.begin();
  bno.setExtCrystalUse(true);
}

//calculer inclinaison
void bno_get_values() {
  imu::Vector<3> gyroscope = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
  roll_speed = gyroscope.x() * 180 / 3.14159265359;
  pitch_speed = gyroscope.y() * 180 / 3.14159265359;
  yaw_speed = gyroscope.z() * 180 / 3.14159265359;

  pid_roll_speed_in = map(roll_speed, -180, 180, ROLL_WMIN, ROLL_WMAX);
  pid_pitch_speed_in = map(pitch_speed, -180, 180, PITCH_WMIN, PITCH_WMAX);
  pid_yaw_speed_in = map(yaw_speed, -180, 180, YAW_WMIN, YAW_WMAX);
}


////fonctions de controle

//calculer l'output et l'ecrire
void control_update() {
  //fait correspondre les valeurs du joystick aux valeurs des moteurs
  throttle = map(input[2], THROTTLE_RMIN, THROTTLE_RMAX, MOTOR_ZERO_LEVEL, MOTOR_MAX_LEVEL);

  //calculer les valeurs a écrire aux moteurs<commit_msg>Added euler angle calculation for AS<commit_after>#include "config.h"
#include <Math.h>
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

//mise en place des variables

Servo motor_right;
Servo motor_left;
Servo motor_front;
Servo motor_back;


// PID variables
bool auto_stabilisation_mode = false; //activer ou pas le mode auto-stabilisé: utiliser le switch de input 5 ou 6
int dt = 3;//notre dt est de 100 millisecondes
double pid_roll_speed_in, pid_roll_angle_in,   pid_roll_out,   pid_roll_setpoint,  roll_error,  Integral_roll_error,  Derivative_roll_error,  last_roll_error, AS_roll_error, AS_Integral_roll_error, AS_Derivative_roll_error, AS_last_roll_error = 0;
double pid_pitch_speed_in, pid_pitch_angle_in,  pid_pitch_out,  pid_pitch_setpoint, pitch_error, Integral_pitch_error, Derivative_pitch_error, last_pitch_error, AS_pitch_error, AS_Integral_pitch_error, AS_Derivative_pitch_error, AS_last_pitch_error = 0;
double pid_yaw_speed_in,    pid_yaw_out,       pid_yaw_setpoint,   yaw_error,   Integral_yaw_error,   Derivative_yaw_error,   last_yaw_error = 0;

// MOTORS
int mR, mL, mF, mB;

//RX
int throttle = THROTTLE_RMIN;
volatile int input[4];
unsigned long timer[4];
byte last_channel[4];

//IMU variables
float roll_speed, roll_angle;
float pitch_speed, pitch_angle;
float yaw_speed, yaw_angle;
Adafruit_BNO055 bno = Adafruit_BNO055();


////fonctions de lecture du gyro et accel

//Initiation du IMU
void bno_initialisation() {
  bno.begin();
  bno.setExtCrystalUse(true);
}

//calculer inclinaison
void bno_get_values() {
  imu::Vector<3> gyroscope = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
  roll_speed = gyroscope.x() * 180 / 3.14159265359;
  pitch_speed = gyroscope.y() * 180 / 3.14159265359;
  yaw_speed = gyroscope.z() * 180 / 3.14159265359;

  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  roll_angle = euler.x();
  pitch_angle = euler.y();

  pid_roll_speed_in = map(roll_speed, -180, 180, ROLL_WMIN, ROLL_WMAX);
  pid_pitch_speed_in = map(pitch_speed, -180, 180, PITCH_WMIN, PITCH_WMAX);
  pid_yaw_speed_in = map(yaw_speed, -180, 180, YAW_WMIN, YAW_WMAX);

  pid_roll_angle_in = map(roll_angle, -180, 180, ROLL_WMIN, ROLL_WMAX);
  pid_pitch_angle_in = map(pitch_angle, -180, 180, PITCH_WMIN, PITCH_WMAX);
}


////fonctions de controle

//calculer l'output et l'ecrire
void control_update() {
  //fait correspondre les valeurs du joystick aux valeurs des moteurs
  throttle = map(input[2], THROTTLE_RMIN, THROTTLE_RMAX, MOTOR_ZERO_LEVEL, MOTOR_MAX_LEVEL);

  //calculer les valeurs a écrire aux moteurs
{'fraction_non_alphanumeric': 0.09892777665385394, 'fraction_numerical': 0.0392474205947805, 'mean_word_length': 5.5570291777188325, 'pattern_counts': {'":': 0, '<': 18, '<?xml version=': 0, '>': 18, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6188303', 'n_tokens_mistral': 2188, 'n_tokens_neox': 1957, 'n_words': 433}