Différences

Ci-dessous, les différences entre deux révisions de la page.

--- inmoov:code:nunchuk [2017/03/01 22:36]
plguen
+++ inmoov:code:nunchuk [2018/05/02 07:15] (Version actuelle)
@@ Ligne 24: / Ligne 24: @@
 int nbServo = 9;
-Servo servo[] = {servothumb,servoindex,servomajeure,servoringfinger,servopinky,servowrist,servoneck,servojaw};
+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, 0, 0, 0};
+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, 90, 30};
+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
@@ Ligne 42: / Ligne 43: @@
 int butonALL=3;
-void moveServo(int joypos, int servoNum, String axe) {
+void moveServo(int joyMin, int joyMax, int joypos, int servoNum, String axe) {
   //=================================================//
   joy = joypos, DEC;
-  Serial.println("joy");Serial.println(axe);
+  // Serial.println("joy");
-  Serial.println(joy);
+  Serial.println(axe);
+  // Serial.println(joy);
   // on limite la valeur entre servoMin et servoMax
-  position = constrain(joy, servoMin[servoNum], servoMax[servoNum]);
+  position = constrain(joy, joyMin, joyMax);
   // on mappe cette valeur pour le servomoteur soit entre servoMin et servoMax
-  position = map(position, servoMin[servoNum], servoMax[servoNum], 0, servoMax[servoNum]);
+  position = map(position, joyMin, joyMax, servoMin[servoNum], servoMax[servoNum]);
-  // on écrit sur le servomoteur la valeur
+  // on écrit sur le servomoteur la val(eur
+  Serial.println(position);
   servo[servoNum].write(position);
+  delay(100);
   //=================================================//
 }
@@ Ligne 80: / Ligne 84: @@
 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 ;
-  //Axe X Tête de Gauche à Droite
-  moveServo(nunchuk.analogX, 8, "X");
-  //Axe Y Tête de Bas en Haut
+  if ((nunchuk.analogX < 125) || (nunchuk.analogX > 133)) {
-  moveServo(nunchuk.analogY, 6, "Y");
+      //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[7].write(servoMax[7]);
+      servo[8].write(servoMax[8]);
     } else {
       Serial.println("Fermeture Bouche");
-      servo[7].write(servoMin[7]);
+      servo[8].write(servoMin[8]);
     }
@@ Ligne 107: / Ligne 133: @@
   if ((nunchuk.accelX < 450) || (nunchuk.accelX > 550)) {
     //Poignet
-    moveServo(nunchuk.accelX, 5, "aX");
+    moveServo(320, 680, nunchuk.accelX, 5, "aX");
   }
   //Fermer la main
-  if (nunchuk.accelY < 450) {
+  if (nunchuk.accelY > 450) {
+    Serial.println(nunchuk.accelY);
     //Main
     for (int thisPin = 0; thisPin < 5; thisPin++) {
-      moveServo(nunchuk.accelY, thisPin, "aY");
+      moveServo(490, 270, 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);
-*/
 }
 </code>

Pahzlg

Outils pour utilisateurs

Outils du site

Différences

Outils de la page