Olá pessoal,
Neste post veremos a implementação dos comandos do trocador de fitas. Através de comandos recebidos pela serial o Arduino aciona os motores e o servo para fazer os movimentos.
// Programa : Tape changer - garra e motores de passo
// Autor : Fernando Asashi Nitatori
#include <Servo.h> // Biblioteca para acionamento do servo
#include <AccelStepper.h> //Biblioteca para os motores de passo
Servo myservo; // Cria um objeto do tipo Servo para controlar um servo chamado myservo
int pos = 0; // variavel para armazenar a posicao do servo
int position;
int positioncenter = 84; // variavel para setar a posicao central do servo
int velocidade_motor_1 = 600; //velocidade do motor de passo 1
int velocidade_motor_2 = 600; //velocidade do motor de passo 1
int aceleracao_motor_1 = 80; //aceleracao do motor de passo 1
int aceleracao_motor_2 = 80; //aceleracao do motor de passo 1
int sentido_horario_motor_1 = 0;
int sentido_antihorario_motor_1 = 0;
int sentido_horario_motor_2 = 0;
int sentido_antihorario_motor_2 = 0;
int chave1;
int chave2;
int chave3;
int pino_endstop1 = 9;
int pino_endstop2 = 10;
int pino_endstop3 = 11;
int posicao = 0;
int posicao2 = 0;
int posicao1_motor1 = 8000;
int posicao2_motor1 = 16000;
int posicao3_motor1 = 24000;
int posicao1_motor2 = 2500;
int posicao2_motor2 = 10000;
int posicao3_motor2 = 22000;
char ultcmdservo = 'm';
char comando;
// Definicao pino ENABLE
int pino_enable = 8;
// Definicao pinos STEP e DIR
AccelStepper motor1(1,2,5);
AccelStepper motor2(1,4,7);
void setup()
{
Serial.begin(9600);
pinMode(pino_enable, OUTPUT);
pinMode(pino_endstop1, INPUT);
pinMode(pino_endstop2, INPUT);
pinMode(pino_endstop3, INPUT);
digitalWrite(pino_endstop1, HIGH);
digitalWrite(pino_endstop2, HIGH);
digitalWrite(pino_endstop3, HIGH);
// Configuracoes iniciais motor de passo
motor1.setEnablePin(pino_enable);
motor1.setMaxSpeed(velocidade_motor_1);
motor1.setSpeed(velocidade_motor_1);
motor1.setAcceleration(aceleracao_motor_1);
// Configuracoes iniciais motor de passo
motor2.setEnablePin(pino_enable);
motor2.setMaxSpeed(velocidade_motor_2);
motor2.setSpeed(velocidade_motor_2);
motor2.setAcceleration(aceleracao_motor_2);
sentido_horario_motor_1 = 0;
sentido_antihorario_motor_1 = 0;
sentido_horario_motor_2 = 0;
sentido_antihorario_motor_2 = 0;
ligaservo();
myservo.write(positioncenter);
Serial.println("Digite l, m ou r para movimentar o servo e clique em ENVIAR...");
Serial.println("Digite c para parar o motor 1...");
Serial.println("Digite f para parar o motor 2...");
Serial.println("Digite 0 para girar o motor 1 para esquerda.");
Serial.println("Digite 1 para girar o motor 1 para a posicao 1.");
Serial.println("Digite 2 para girar o motor 1 para a posicao 2.");
Serial.println("Digite 3 para girar o motor 1 para a posicao 3.");
Serial.println("Digite 5 para girar o motor 2 para esquerda.");
Serial.println("Digite 6 para girar o motor 2 para a posicao 6.");
Serial.println("Digite 7 para girar o motor 2 para a posicao 7.");
//Serial.println("Digite 8 para girar o motor 2 para a posicao 8.");
}
void ligaservo() {
myservo.attach(19,500,2700); // attaches the servo on pin 19 to the servo object
}
void desligaservo() {
myservo.detach(); //desliga o servo
}
/*
void movezero() {
posicao = 0;
moveposicao0motor1();
}
*/
void moveposicao0motor1() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_1 = 0;
sentido_antihorario_motor_1 = 1;
}
void moveposicao0motor2() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_2 = 1;
sentido_antihorario_motor_2 = 0;
}
void moveposicao1motor2() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_2 = 0;
sentido_antihorario_motor_2 = 1;
posicao2 = 1;
}
void moveposicao2motor2() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_2 = 0;
sentido_antihorario_motor_2 = 1;
posicao2 = 2;
}
void moveposicao3motor2() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_2 = 0;
sentido_antihorario_motor_2 = 1;
posicao2 = 3;
}
void moveposicao1motor1() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_1 = 1;
sentido_antihorario_motor_1 = 0;
posicao = 1;
}
void moveposicao2motor1() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_1 = 1;
sentido_antihorario_motor_1 = 0;
posicao = 2;
}
void moveposicao3motor1() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_1 = 1;
sentido_antihorario_motor_1 = 0;
posicao = 3;
}
void ligamotor1horario() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_1 = 1;
sentido_antihorario_motor_1 = 0;
}
void ligamotor1antihorario() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_1 = 0;
sentido_antihorario_motor_1 = 1;
}
void ligamotor2horario () {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_2 = 1;
sentido_antihorario_motor_2 = 0;
}
void ligamotor2antihorario() {
digitalWrite(pino_enable, LOW);
sentido_horario_motor_2 = 0;
sentido_antihorario_motor_2 = 1;
// puxa_horario_motor_2 = 0;
// puxa_antihorario_motor_2 = 0;
}
void loop()
{
// Aguarda os caracteres na serial
if (Serial.available() > 0)
{
comando = (char) Serial.read();
if (comando == 'c')
{
Serial.println("Letra c recebido - Parando motor 1...");
sentido_horario_motor_1 = 0;
sentido_antihorario_motor_1 = 0;
motor1.stop();
motor1.runToPosition();
motor1.setCurrentPosition(0);
digitalWrite(pino_enable, HIGH);
}
if (comando == 'f')
{
Serial.println("Letra f recebido - Parando motor 2...");
sentido_horario_motor_2 = 0;
sentido_antihorario_motor_2 = 0;
motor2.stop();
motor2.runToPosition();
motor2.setCurrentPosition(0);
digitalWrite(pino_enable, HIGH);
}
if (comando == 'l') {
ligaservo();
if (ultcmdservo == 'r') {
Serial.println("Nao digitar comando r aqui. Va para o centro com m e depois va para direita com r");
}
else
{
for(pos = positioncenter; pos<=162; pos += 1)
{
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(50);
}
}
ultcmdservo = 'l';
desligaservo();
}
if (comando == 'm') {
ligaservo();
position = myservo.read();
//Serial.println(position);
if (position >= positioncenter) {
for(pos = position; pos>=positioncenter; pos -= 1) // goes from 0 degrees to 180 degrees
{
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(50); // waits 15ms for the servo to reach the position
}
}
else
{
for(pos = position; pos<=positioncenter; pos += 1) // goes from 0 degrees to 180 degrees
{ // in steps of 1 degree
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(50); // waits 15ms for the servo to reach the position
}
}
ultcmdservo = 'm';
desligaservo();
}
if (comando == 'r') {
ligaservo();
if (ultcmdservo == 'l') {
Serial.println("Nao digitar comando l aqui. Va para o centro com m e depois va para direita com l");
}
else
{
for(pos = positioncenter; pos>=5; pos-=1) {
myservo.write(pos); // movimenta o servo ate a posicao
delay(50); // espera 50ms para o servo atingir a posicao
}
}
ultcmdservo = 'r';
desligaservo();
}
if (comando == '0') {
moveposicao0motor1();
}
if (comando == '1') {
moveposicao1motor1();
}
if (comando == '2') {
moveposicao2motor1();
}
if (comando == '3') {
moveposicao3motor1();
}
if (comando == '5') {
moveposicao0motor2();
}
if (comando == '6') {
moveposicao1motor2();
}
if (comando == '7') {
moveposicao2motor2();
}
if (comando == '8') {
moveposicao3motor2();
}
}
chave1 = digitalRead(pino_endstop1);
chave2 = digitalRead(pino_endstop2);
chave3 = digitalRead(pino_endstop3);
// Move o motor1 no sentido horario ate a posicao desejada
if (sentido_horario_motor_1 == 1){
if (posicao == 1) {
motor1.moveTo(posicao1_motor1);
motor1.run();
}
if (posicao == 2) {
motor1.moveTo(posicao2_motor1);
motor1.run();
}
if (posicao == 3) {
motor1.moveTo(posicao3_motor1);
motor1.run();
}
}
// Move o motor1 no sentido anti-horario ate a posicao desejada
if (sentido_antihorario_motor_1 == 1){
if (posicao == 0) {// se posicao for 0 move ate encontrar chave fim de curso
if (chave1 == 0) {
Serial.println("Fim de curso - Parando motor 1...");
sentido_horario_motor_1 = 0;
sentido_antihorario_motor_1 = 0;
motor1.stop();
motor1.runToPosition();
// motor1.setCurrentPosition(0);
digitalWrite(pino_enable, HIGH);
}
else
{
motor1.move(-20000);
motor1.run();
}
}
motor1.moveTo(0);
motor1.run();
}
// Move o motor2 no sentido horario ate a posicao desejada
if (sentido_horario_motor_2 == 1){
if (posicao2 == 0) {
if (chave2 == 0) {
Serial.println("Fim de curso 2 - Parando motor 2...");
sentido_horario_motor_2 = 0;
sentido_antihorario_motor_2 = 0;
motor2.stop();
motor2.runToPosition();
digitalWrite(pino_enable, HIGH);
}
else
{
motor2.move(-19000);
motor2.run();
}
}
motor2.moveTo(0);
motor2.run();
}
// Move o motor2 no sentido anti-horario ate a posicao desejada
if (sentido_antihorario_motor_2 == 1) {
if (posicao2 == 1) {
motor2.moveTo(posicao1_motor2);
motor2.run();
}
if (posicao2 == 2) {
motor2.moveTo(posicao2_motor2);
motor2.run();
}
if (posicao2 == 3) {
motor2.moveTo(posicao3_motor2);
motor2.run();
}
}
}
O código acima é para a implementação do movimento da garra e servo. Segue abaixo código para o motor DC:
//Programa: Tape changer - abre e fecha a garra
#include <AFMotor.h>
AF_DCMotor motor(1);
char comando;
void abregarra() {
uint8_t i;
motor.run(BACKWARD);
for (i=0; i<128; i++) {
motor.setSpeed(i);
delay(10);
}
delay(3000);
motor.run(RELEASE);
}
void fechagarra() {
uint8_t i;
motor.run(FORWARD);
for (i=0; i<255; i++) {
motor.setSpeed(i);
delay(10);
}
delay(2000);
}
void setup () {
Serial.begin(9600);
}
void loop () {
if (Serial.available() > 0)
{
comando = (char) Serial.read();
if (comando == 'g')
{
Serial.println("Abrindo garra");
abregarra();
}
if (comando == 'h')
{
Serial.println("Fechando garra");
fechagarra();
}
}
}
O script retirafita.sh é um script que roda em shell Linux ou FreeBSD. Segue abaixo script completo com os comandos para retirar a fita do drive e colocá-la no compartimento:
#Script retira.sh
echo "Abre a garra..."
echo 'g' > /dev/tty.usbmodem1421
sleep 5s
echo "Fecha a garra..."
echo 'h' > /dev/tty.usbmodem1421
sleep 5s
echo "Movimenta a garra ate a posicao 8..."
echo '8' > /dev/tty.usbmodem1411
sleep 45s
echo "Vira para a direita..."
echo 'r' > /dev/tty.usbmodem1411
sleep 5s
echo "Movimenta a garra ate a posicao 5..."
echo '5' > /dev/tty.usbmodem1411
sleep 45s
echo "Abre a garra..."
echo 'g' > /dev/tty.usbmodem1421
sleep 5s
echo "Movimenta a garra ate a posicao 7..."
echo '7' > /dev/tty.usbmodem1411
sleep 22s
echo "Fecha a garra..."
echo 'h' > /dev/tty.usbmodem1421
sleep 5s
echo "Movimenta a garra ate a posicao 5..."
echo '5' > /dev/tty.usbmodem1411
sleep 27s
echo "Movimenta a garra ate a posicao 6..."
echo '6' > /dev/tty.usbmodem1411
sleep 10s
echo "Vai para o centro..."
echo 'm' > /dev/tty.usbmodem1411
sleep 5s
echo "Abre a garra..."
echo 'g' > /dev/tty.usbmodem1421
sleep 5s
echo "Movimenta a garra ate a posicao 5..."
echo '5' > /dev/tty.usbmodem1411
sleep 5s
Quaisquer dúvidas sobre os códigos entrem em contato,
Até mais pessoal,
Nenhum comentário:
Postar um comentário