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,servojaw};
int servoPin[] = {13, 12, 11, 10, 9, 8, 14, 15, 16};
int servoInit[] = {0, 0, 0, 0, 0, 90, 0, 0, 0};
int servoMin[] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
int servoMax[] = {130, 150, 140, 140, 125, 45, 120, 90, 30};

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 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, servoMin[servoNum], servoMax[servoNum]);
  
  // on mappe cette valeur pour le servomoteur soit entre servoMin et servoMax
  position = map(position, servoMin[servoNum], servoMax[servoNum], 0, servoMax[servoNum]);
  
  // on écrit sur le servomoteur la valeur
  servo[servoNum].write(position);

  //=================================================//
}


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
  
  nunchuk.update();

  zButtonState = nunchuk.zButton ;
  
  //Axe X Tête de Gauche à Droite
  moveServo(nunchuk.analogX, 8, "X");

  //Axe Y Tête de Bas en Haut
  moveServo(nunchuk.analogY, 6, "Y");

  if (zButtonState != lastZButtonState) {
    //Axe Y Ouverture Bouche
    if(zButtonState == 1){
      Serial.println("Ouverture Bouche");
      servo[7].write(servoMax[7]);       
    } else {
      Serial.println("Fermeture Bouche");
      servo[7].write(servoMin[7]);
    }

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

  //Fermer la main
  if (nunchuk.accelY < 450) {
    //Main
    for (int thisPin = 0; thisPin < 5; thisPin++) {
      moveServo(nunchuk.accelY, thisPin, "aY");
    }  
  }
  
  delay(100);
  
  if(digitalRead(buton0)==HIGH){
     
    alltovirtual();
    //delay(2000);
  } else if(digitalRead(buton90)==HIGH){ 
    
    allto90(); 
    //delay(2000);
  } else if(digitalRead(butonALL)==HIGH){ 
    
    alltomax(); 
    //delay(2000);
  }


}
// Motion to set the servo into "virtual" 0 position: alltovirtual
void alltovirtual() {
servothumb.write(0);
servoindex.write(0);
servomajeure.write(0);
servoringfinger.write(0);
servopinky.write(0);
servowrist.write(90);
/*servobiceps.write(0);
servorotate.write(20); //Never less then (20 degree)
servoshoulder.write(30); //Never less then (30 degree)
servoomoplat.write(10); //Never less then (10 degree)
servoneck.write(0);
servorothead.write(0);*/
}

// Motion to set the servo into "rest" position: alltorest
void alltorest() {
servothumb.write(0);
servoindex.write(0);
servomajeure.write(0);
servoringfinger.write(0);
servopinky.write(0);

servowrist.write(0);
/*
servobiceps.write(0);
servorotate.write(90); //Never less then (20 degree)
servoshoulder.write(30); //Never less then (30 degree)
servoomoplat.write(10); //Never less then (10 degree)
servoneck.write(90);
servorothead.write(90);*/
}

// Motion to set the servo into "max" position: alltomax
void alltomax() {

servothumb.write(130);
servoindex.write(150);
servomajeure.write(140);
servoringfinger.write(140);
servopinky.write(125);
servowrist.write(45);
/*
 * 
servobiceps.write(85); //Never more then (85 or 90degree)
servorotate.write(110); //Never more then (110 degree)
servoshoulder.write(130); //Never more then (130 degree)
servoomoplat.write(70); //Never more then (70 degree)
servoneck.write(180);
servorothead.write(180);
*/
}

// Motion to set the servo into "90" position: allto90
void allto90() {
servothumb.write(80);
servoindex.write(90);
servomajeure.write(90);
servoringfinger.write(85);
servopinky.write(90);
servowrist.write(50);
/*
servobiceps.write(90); //Never more then (85 or 90degree)
servorotate.write(90); //Never more then (110 degree)
servoshoulder.write(90); //Never more then (130 degree)
servoomoplat.write(70); //Never more then (70 degree)
servoneck.write(90);
servorothead.write(180);
*/
}