Quel est cet article?

Je développe actuellement un robot qui résout un cube rubic 2x2x2. Ceci est une collection d'articles de commentaires sur le programme du robot. J'ai écrit une fois une collection d'articles représentés par l'article ici, mais depuis cette fois, le logiciel a été considérablement mis à jour, je vais donc introduire un nouveau programme. pense.

Le code applicable est disponible ici [https://github.com/Nyanyan/soltvvo/tree/master/Soltvvo3.2).

Constantes et variables

Constantes et variables placées globalement.

const int magnet_threshold = 50;
const long turn_steps = 400;
const int step_dir[2] = {11, 9};
const int step_pul[2] = {12, 10};
const int sensor[2] = {14, 15};
const int grab_deg[2] = {74, 74};
const int release_deg[2] = {96, 96};
const int offset = 3;

char buf[30];
int idx = 0;
long data[3];

La constante est le numéro de port ou l'angle du servomoteur. Les variables sont utilisées pour diviser les valeurs données en les séparant par des espaces.

Autour de la bibliothèque de servo

Incluez la bibliothèque de servo en relation avec l'utilisation du servomoteur et définissez `` servo0, servo1 ''.

#include <Servo.h>

Servo servo0;
Servo servo1;

installer

Installer.

void setup() {
  Serial.begin(115200);
  for (int i = 0; i < 2; i++) {
    pinMode(step_dir[i], OUTPUT);
    pinMode(step_pul[i], OUTPUT);
    pinMode(sensor[i], INPUT);
  }
  servo0.attach(7);
  servo1.attach(8);
  servo0.write(release_deg[0] + 5);
  servo1.write(release_deg[1] + 5);
  delay(70);
  servo0.write(release_deg[0]);
  servo1.write(release_deg[1]);
}

Définissez les entrées et sorties des broches et déplacez le servomoteur pour déplacer le bras vers l'extérieur.

Faites pivoter le bras

C'est une fonction qui fait tourner le moteur pas à pas. J'ai implémenté un entraînement trapézoïdal, mais pour cette raison, c'est une implémentation légèrement peu cool qui utilise `` retard '' sans utiliser de minuteries, etc.

void move_motor(long num, long deg, long spd) {
  bool hl = true;
  if (deg < 0) hl = false;
  digitalWrite(step_dir[num], hl);
  long steps = abs(deg) * turn_steps / 360;
  long avg_time = 1000000 * 60 / turn_steps / spd;
  long max_time = 1500;
  long slope = 50;
  bool motor_hl = false;
  long accel = min(steps / 2, max(0, (max_time - avg_time) / slope));
  int num1 = (num + 1) % 2;
  //accélération
  for (int i = 0; i < accel; i++) {
    motor_hl = !motor_hl;
    digitalWrite(step_pul[num], motor_hl);
    delayMicroseconds(max_time - slope * i);
  }
  //Fonctionnement normal
  for (int i = 0; i < steps * 2 - accel * 2; i++) {
    motor_hl = !motor_hl;
    digitalWrite(step_pul[num], motor_hl);
    delayMicroseconds(avg_time);
  }
  //Ralentir
  for (int i = 0; i < accel; i++) {
    motor_hl = !motor_hl;
    digitalWrite(step_pul[num], motor_hl);
    delayMicroseconds(max_time - slope * accel + accel * (i + 1));
  }
}

Fondamentalement, il est traité selon le flux suivant.

Calculez le nombre de pas pour tourner à partir de l'angle
Calculez la largeur d'impulsion à partir du nombre de tours
Calculez le nombre d'étapes utilisées pour l'accélération et la décélération

Calibration du bras

Calibrez la partie du bras qui utilise le moteur pas à pas afin qu'elle ait un angle approprié. À ce stade, utilisez l'aimant fixé sur le côté du bras et le capteur à effet Hall (capteur magnétique) fixé sur le côté du moteur pas à pas.

void motor_adjust(long num, long spd) {
  int max_step = 150;
  int delay_time = 800;
  bool motor_hl = false;
  digitalWrite(step_dir[num], LOW);
  while (analogRead(sensor[num]) <= magnet_threshold) {
    motor_hl = !motor_hl;
    digitalWrite(step_pul[num], motor_hl);
    delayMicroseconds(delay_time);
  }
  while (analogRead(sensor[num]) > magnet_threshold) {
    motor_hl = !motor_hl;
    digitalWrite(step_pul[num], motor_hl);
    delayMicroseconds(delay_time);
  }
}

La première déclaration `` while '' est délicate, mais même s'il est déjà déterminé que le bras est dans la bonne position, l'angle du bras peut être légèrement décalé, donc le bras est décalé une fois. ..

La deuxième instruction `` while '' fait tourner le moteur pas à pas jusqu'à ce que l'aimant soit devant le capteur à effet Hall.

Libérez le puzzle

Une fonction qui déplace le bras vers l'extérieur et libère le puzzle.

void release_arm(int num) {
  if (num == 0)servo0.write(release_deg[num] + offset);
  else servo1.write(release_deg[num] + offset);
}

Ce n'est pas une très belle façon d'écrire. Si vous savez comment écrire magnifiquement, faites-le moi savoir. Pour offset```, j'ai implémenté un dépassement pour déplacer le moteur plus rapidement, mais j'ai trouvé qu'il n'était pas nécessaire de revenir à la valeur cible lors du dépassement, j'ai donc ajouté offset``` Il est devenu.

Prenez le puzzle

C'est une fonction qui déplace le bras autour du centre et saisit le puzzle.

void grab_arm(int num) {
  if (num == 0)servo0.write(grab_deg[num] - offset);
  else servo1.write(grab_deg[num] - offset);
  delay(70);
  if (num == 0)servo0.write(grab_deg[num]);
  else servo1.write(grab_deg[num]);
}

Le dépassement créé avec offset est utile ici. J'espère que cela vous aidera à saisir le puzzle plus rapidement (je ne sais pas à quel point il est vraiment efficace)

Boucle principale

La boucle principale reçoit la commande et appelle la fonction correspondante.

void loop() {
  while (1) {
    if (Serial.available()) {
      buf[idx] = Serial.read();
      if (buf[idx] == '\n') {
        buf[idx] = '\0';
        data[0] = atoi(strtok(buf, " "));
        data[1] = atoi(strtok(NULL, " "));
        data[2] = atoi(strtok(NULL, " "));
        if (data[1] == 1000) grab_arm(data[0]);
        else if (data[1] == 2000) release_arm(data[0]);
        else if (data[1] == 0) motor_adjust(data[0], data[2]);
        else move_motor(data[0], data[1], data[2]);
        idx = 0;
      }
      else {
        idx++;
      }
    }
  }
}

La fonction à appeler dépend du type de commande.

Résumé

Cette fois, j'ai présenté le programme Arduino qui déplace réellement l'actionneur. Je pense que c'est en partie impur, probablement à cause de mon manque de connaissance de la langue. Veuillez nous indiquer une bonne manière d'écrire dans les commentaires.

La prochaine fois, j'expliquerai le traitement principal de Python.

[PYTHON] Logiciel mis à jour pour Rubik Cube Robot 6. Fonctionnement de la machine (Arduino)