Pahzlg

Code pour piloter inmoov (Tête et main) avec le nunchuk

//Bibliothèques
#include <Servo.h>
#include <Wire.h>
#include <ArduinoNunchuk.h>

#define BAUDRATE 19200

//Servos
Servo servothumb; //0
Servo servoindex; //1
Servo servomajeure;//2
Servo servoringfinger;//3
Servo servopinky;//4
Servo servowrist;//5
Servo servoneck;//6
Servo servorothead;//7
Servo servojaw;//8

int nbServo = 9;
Servo servo[] = {servothumb,servoindex,servomajeure,servoringfinger,servopinky,servowrist,servoneck,servorothead,servojaw};
int servoPin[] = {13, 12, 11, 10, 9, 8, 14, 15, 16};
int servoInit[] = {0, 0, 0, 0, 0, 90, 60, 60, 0};
int servoMin[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int servoMax[] = {130, 150, 140, 140, 125, 45, 120, 120, 60};

bool initialisation = true;
int position;
int zButtonState = 0;     // Variable pour l'état précédent du bouton poussoir
int lastZButtonState = 0;     // Variable pour l'état précédent du bouton poussoir

ArduinoNunchuk nunchuk = ArduinoNunchuk();
int joy,joyX,joyY;

//Button platine
int buton0=1;
int buton90=2;
int butonALL=3;

void moveServo(int joyMin, int joyMax, int joypos, int servoNum, String axe) {
  //=================================================//
  
  joy = joypos, DEC;
  // Serial.println("joy");
  Serial.println(axe);
  // Serial.println(joy);
  
  // on limite la valeur entre servoMin et servoMax
  position = constrain(joy, joyMin, joyMax); 
 
  // on mappe cette valeur pour le servomoteur soit entre servoMin et servoMax
  position = map(position, joyMin, joyMax, servoMin[servoNum], servoMax[servoNum]);
 
  // on écrit sur le servomoteur la val(eur
  Serial.println(position);
  servo[servoNum].write(position);

  delay(100);
  //=================================================//
}


void setup() {

  //Initialisation des servos
  for (int thisPin = 0; thisPin < nbServo; thisPin++) {
    servo[thisPin].attach(servoPin[thisPin]); //
    servo[thisPin].write(servoInit[thisPin]); //
  }
 
  //Iinitialisation des bouton
  pinMode(buton0,INPUT);
  pinMode(buton90,INPUT);
  pinMode(butonALL,INPUT);
  
  Serial.begin(BAUDRATE); //Initialisation du terminal serie
  nunchuk.init();         //Initialisation du nunchuk
}

void loop() { // Loop through motion tests

  if (initialisation) {
    //Initialisation des servos
    for (int thisPin = 0; thisPin < nbServo; thisPin++) {
      
      servo[thisPin].write(servoInit[thisPin]); //
    }
 
    initialisation = false;
  }
  
  nunchuk.update();

  zButtonState = nunchuk.zButton ;

  if ((nunchuk.analogX < 125) || (nunchuk.analogX > 133)) {
      //Axe X Tête de Gauche à Droite
   //Serial.println("analogX" ); Serial.println(nunchuk.analogX );
    moveServo(30, 227, nunchuk.analogX, 7, "X");
  }

  if ((nunchuk.analogY < 115) || (nunchuk.analogY > 130)) {
    //Axe Y Tête de Bas en Haut
    moveServo(26, 214, nunchuk.analogY, 6, "Y");
  }
  
  if(nunchuk.cButton == 1){
    Serial.println(servoInit[6]);
    servo[6].write(servoInit[6]);
    Serial.println(servoInit[7]);
    servo[7].write(servoInit[7]);  
  }
  
  if (zButtonState != lastZButtonState) {
    //Axe Y Ouverture Bouche
    if(zButtonState == 1){
      Serial.println("Ouverture Bouche");
      servo[8].write(servoMax[8]);       
    } else {
      Serial.println("Fermeture Bouche");
      servo[8].write(servoMin[8]);
    }

    lastZButtonState = zButtonState;
  }
  
  //Tourner le poignet
  if ((nunchuk.accelX < 450) || (nunchuk.accelX > 550)) {
    //Poignet
    moveServo(320, 680, nunchuk.accelX, 5, "aX");
  }

  //Fermer la main
  if (nunchuk.accelY > 450) {
    Serial.println(nunchuk.accelY);
    //Main
    for (int thisPin = 0; thisPin < 5; thisPin++) {
      moveServo(490, 270, nunchuk.accelY, thisPin, "aY");
    }  
  }
  
  if(digitalRead(buton0)==HIGH){
      
  } else if(digitalRead(buton90)==HIGH){ 
    
  } else if(digitalRead(butonALL)==HIGH){ 
    
  }
}