Partie récepteur :
//moteur shield
int MAX_SPEED=150;
const int directionA=12;
const int directionB=13;
const int brakeA=9;
const int brakeB=8;
const int speedA=3;
const int speedB=11;
const int in2=A2;
const int in3=A3;
//ports disponibles : 0, 1, 2, 4, 5, 6, 7, 10, A0, A1, A4, A5
//servomoteurs
#include <ServoTimer2.h>
ServoTimer2 myservo_droit;
ServoTimer2 myservo_gauche;
const int servo_droit=xxx;
const int servo_gauche=xxx;
bool initservo=true;
//récepteur
#include <VirtualWire.h>
const int recepteur=xxx;
//capteur à ultrason
#include "SR04.h"
#define TRIG_PIN xxx
#define ECHO_PIN xxx
SR04 sr04 = SR04(ECHO_PIN,TRIG_PIN);
long altitude_ref;
long altitude;
//gyroscope
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "math.h"
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
uint8_t Accel_range;
uint8_t Gyro_range;
float angley=0;
float anglex=0;
//commandes
const char* CMD_BUTTON_M="BPM";
const char* CMD_BUTTON_D="BPD";
const char* EQUILIBRE="ECQ";
const char* AVANT="AVA";
const char* ARRIERE="ARR";
const char* DROITE="DRO";
const char* GAUCHE="GAU";
const char* AVANT_MAX="AVM";
const char* ARRIERE_MAX="ARM";
const char* DROITE_MAX="DRM";
const char* GAUCHE_MAX="GAM";
const char* DEMARRAGE="DEM";
const char* ARRET="ARE";
const char* STATIQUE="STA";
const char* SORTIE="SOR";
void setup() {
//moteurs
pinMode(directionA, OUTPUT);
pinMode(brakeA, OUTPUT);
pinMode(directionB, OUTPUT);
pinMode(brakeB, OUTPUT);
//servomoteurs
myservo_droit.attach(servo_droit);
myservo_gauche.attach(servo_gauche);
//récepteur
vw_set_rx_pin(recepteur);
vw_setup(2000);
vw_rx_start();
//gyroscope
Wire.begin();
accelgyro.initialize();
}
void loop() {
//données à recevoir
byte message[VW_MAX_MESSAGE_LEN];
byte taille_message = VW_MAX_MESSAGE_LEN;
int valeurRecue;
byte taille_message2=sizeof(valeurRecue);
vw_wait_rx();
if(vw_get_message(message, &taille_message)) {
//quand on reçoit un message
if(strcmp((char*) message, DEMARRAGE) == 0) {
//allumage moteurs
if(initservo==true) {
//remise des pales à l'horizontale
initservo=false;
myservo_droit.write(90);
myservo_gauche.write(90);
delay(500);
}
dcForward();
}
if(strcmp((char*) message, ARRET) == 0) {
//extinction moteurs
initservo=true;
dcStop();
int MAX_SPEED=150;
}
if(strcmp((char*) message, CMD_BUTTON_M) == 0) {
//l'hélicoptère monte
int MAX_SPEED=255;
dcForward();
delay(100);
int MAX_SPEED=150;
dcForward();
}
if(strcmp((char*) message, CMD_BUTTON_D) == 0) {
//l'hélicoptère descend
int MAX_SPEED=100;
dcForward();
delay(100);
int MAX_SPEED=150;
dcForward();
}
if(strcmp((char*) message, AVANT) == 0) {
//l'hélicoptère va vers l'avant
vw_wait_rx();
if(vw_get_message((byte*) &valeurRecue, &taille_message2)) {
myservo_droit.write(90+(505-valeurRecue)*0.04);
myservo_gauche.write(90-(505-valeurRecue)*0.04);
}
}
if(strcmp((char*) message, ARRIERE) == 0) {
//l'hélicoptère va vers l'arrière
vw_wait_rx();
if(vw_get_message((byte*) &valeurRecue, &taille_message2)) {
myservo_droit.write(90-(valeurRecue-507)*0.04);
myservo_gauche.write(90+(valeurRecue-507)*0.04);
}
}
if(strcmp((char*) message, EQUILIBRE) == 0) {
//l'hélicoptère reste à l'horizontale
myservo_droit.write(90);
myservo_gauche.write(90);
}
if(strcmp((char*) message, AVANT_MAX) == 0) {
//l'hélicoptère va vers l'avant au maximum
myservo_gauche.write(70);
myservo_droit.write(110);
}
if(strcmp((char*) message, ARRIERE_MAX) == 0) {
//l'hélicoptère va vers l'arrière au maximum
myservo_droit.write(70);
myservo_gauche.write(110);
}
if(strcmp((char*) message, DROITE) == 0) {
//l'hélicoptère va vers la droite
vw_wait_rx();
if(vw_get_message((byte*) &valeurRecue, &taille_message2)) {
myservo_droit.write(90-(505-valeurRecue)*0.04);
myservo_gauche.write(90-(505-valeurRecue)*0.04);
}
}
if(strcmp((char*) message, GAUCHE) == 0) {
//l'hélicoptère va vers la gauche
vw_wait_rx();
if(vw_get_message((byte*) &valeurRecue, &taille_message2)) {
myservo_droit.write(90+(valeurRecue-507)*0.04);
myservo_gauche.write(90+(valeurRecue-507)*0.04);
}
}
if(strcmp((char*) message, DROITE_MAX) == 0) {
//l'hélicoptère va vers la droite au maximum
myservo_droit.write(70);
myservo_gauche.write(70);
}
if(strcmp((char*) message, GAUCHE_MAX) == 0) {
//l'hélicoptère va vers la gauche au maximum
myservo_droit.write(110);
myservo_gauche.write(110);
}
if(strcmp((char*) message, STATIQUE) == 0) {
//l'hélicoptère est en régime statique
altitude_ref=sr04.Distance();
while(strcmp((char*) message, SORTIE) != 0) {
//on reste en régime statique tant que l'ordre d'en sortir n'a pas été donné
vw_wait_rx();
altitude=sr04.Distance();
if(altitude<altitude_ref-10) {
//il faut augmenter l'altitude
...
}
if(altitude>altitude_ref+10) {
//il faut réduire l'altitude
...
}
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
angley=0.98*(angley+float(gy)*0.01/131) + 0.02*atan2((double)ax,(double)az)*180/PI;
anglex=0.98*(anglex+float(gx)*0.01/131) + 0.02*atan2((double)ay,(double)az)*180/PI;
if(angley<0) {
//il faut redresser l'appareil
...
}
if(angley>0) {
//il faut redresser l'appareil
...
}
if(anglex<0) {
//il faut redresser l'appareil
...
}
if(anglex>0) {
//il faut redresser l'appareil
...
}
delay(10);
}
}
}
}
void dcForward() {
//fonctionnement des moteurs quand ils sont allumés
digitalWrite(directionA, HIGH);
digitalWrite(brakeA, LOW);
digitalWrite(directionB, HIGH);
digitalWrite(brakeB, LOW);
analogWrite(speedA, MAX_SPEED);
analogWrite(speedB, MAX_SPEED);
}
void dcStop() {
//arrêt des moteurs
digitalWrite(brakeA, HIGH);
analogWrite(speedA, 0);
digitalWrite(brakeB, HIGH);
analogWrite(speedB, 0);
}