Um dia pensei em fazer um sistema arduino, pan-tilt para fixar minha gopro, utilizando 2 servos Turnigy 9g.
Após algumas horas pesquisando na net, notei que para conseguir mover os motores ao mesmo tempo e com velocidades diferentes,
deveria utilizar as interrupções do processador em meu favor.
O objetivo deste post é mostrar uma forma de efetuar tais movimentos, através de comandos enviados por monitor serial, porém,
meu projeto final será controlá-los através de um APP android, conectados por bluetooth. Estas melhorias serão reportadas
posteriormente. Neste primeiro momento, o mais importante é notar que ao invés de usarmos delay() para controlar a velocidade,
ou então, para mover o motor, utilizamos o próprio método loop().
/**
How to move 2 independent servos
Command: Pattern: Usage:
Speed S[X-Y][1-9]$ SX9$ = S ->speed | X/Y -> axis | 1 - 5 -> 1 faster - 5 slower | $ -> end command
Movement [X-Y][0-9]$ X0$ = move from 0 to 180 (0 to 9 position) degree
Reset R$ R$ = reset to default position (90 degree)
Pause/resume P[X-Y]$ PX$
Cancel C[X-Y]$ CX$ = cancel the movement
**/
#include "Servo.h"
//Servo pins
Servo servoX;
Servo servoY;
int const servoXpin = 3;
int const servoYpin = 4;
int const serialPort = 9600;
//Controlling input commands
char inputText; //read input commands
String textConcat = " "; //concatenated string from imput commands
//Controll default values (speed, axis, rotation)
long const defaultSpeed = 10000; //100 * 1 to 9 (faster to slower) in ms
int const defaultPosition = 90; //inicial angle
//Setting XY axis inicial values
long currentSpeedX = defaultSpeed;
long currentSpeedY = defaultSpeed;
long delaySpeedX = 0;
long delaySpeedY = 0;
int pauseX = 0;
int pauseY = 0;
int destinationPositionX = defaultPosition;
int destinationPositionY = defaultPosition;
int currentPositionX = defaultPosition;
int currentPositionY = defaultPosition;
void setup() {
//start serial
Serial.begin(serialPort);
//atach servos
servoX.attach(servoXpin);
servoY.attach(servoYpin);
//defaultPosition for both servos
resetPosition();
}
/**
Go to position 90 degrees (both axis)
**/
void resetPosition() {
currentPositionX = defaultPosition;
currentPositionY = defaultPosition;
destinationPositionX = defaultPosition; //reset counter if moving
destinationPositionY = defaultPosition;
servoX.write(defaultPosition);
servoY.write(defaultPosition);
}
/**
Set new speed for axis
**/
void setNewSpeed(char axis, int newSpeed) {
long val = long(newSpeed) - 48;
long calculatedSpeed = (long)defaultSpeed * val;
//Console messages
/**/
Serial.print("Setting new speed for axis: ");
Serial.print(axis);
Serial.print(" to speed: ");
Serial.print(long(calculatedSpeed));
Serial.print(" : ");
Serial.print(defaultSpeed);
Serial.print(" : ");
Serial.print(val);
Serial.println();
/**/
if (axis == 'X') {
currentSpeedX = calculatedSpeed;
}
else if (axis == 'Y') {
currentSpeedY = calculatedSpeed;
}
}
/**
Setter the values to move the servos
**/
void setMovement(char axis, int newPosition) {
if (axis == 'X') {
destinationPositionX = newPosition;
}
else if (axis == 'Y') {
destinationPositionY = newPosition;
}
}
/**
Verify if servos need to move and move then, with specif speed
This method is called every loop
**/
void moveAxisXY() {
//moving X
if (destinationPositionX != currentPositionX && pauseX == 0) {
int ascendDescendX = destinationPositionX > currentPositionX ? 1 : 0; //1 = ascending - 0 = descending
delaySpeedX++;
if (delaySpeedX == currentSpeedX) {
delaySpeedX = 0;
currentPositionX = ascendDescendX == 0 ? currentPositionX - 1 : currentPositionX + 1;
servoX.write(currentPositionX);
//Console messages
/**/
Serial.print("Moving servo from ");
Serial.print(currentPositionX);
Serial.print(" to ");
Serial.print(destinationPositionX);
Serial.print(" with speed ");
Serial.print(currentSpeedX);
Serial.println();
/**/
}
}
//moving Y
if (destinationPositionY != currentPositionY && pauseY == 0) {
int ascendDescendY = destinationPositionY > currentPositionY ? 1 : 0; //1 = ascending - 0 = descending
delaySpeedY++;
if (delaySpeedY == currentSpeedY) {
delaySpeedY = 0;
currentPositionY = ascendDescendY == 0 ? currentPositionY - 1 : currentPositionY + 1;
servoY.write(currentPositionY);
//Console messages
/**/
Serial.print("Moving servo from ");
Serial.print(currentPositionY);
Serial.print(" to ");
Serial.print(destinationPositionY);
Serial.print(" with speed ");
Serial.print(currentSpeedY);
Serial.println();
/**/
}
}
}
/**
Process the input following this instructions:
**/
void processInput() {
inputText = char(Serial.read());
/** /
Serial.print(int(inputText));
Serial.println();
Serial.println();
/**/
if (inputText != '\n' && inputText != '$') {
textConcat += inputText;
}
else if (inputText == '$') {
//setting speed -> SX1$
if (textConcat[1] == 'S') {
char axis = textConcat[2];
int newSpeed = int(textConcat[3]);
setNewSpeed(axis, newSpeed);
}
//setting axis position
else if (textConcat[1] == 'X' || textConcat[1] == 'Y') {
char axis = textConcat[1];
char strPosition1 = textConcat[2];
int newPosition = (int(strPosition1)-48)*20; // - 48);
//Console messages
/**/
Serial.print("Setting new position for ");
Serial.print(axis);
Serial.print(" axis: ");
Serial.print(newPosition);
Serial.println();
setMovement(axis, newPosition);
/**/
}
//pausing movement
else if (textConcat[1] == 'P') {
if (textConcat[2] == 'X') {
pauseX = pauseX == 0 ? 1 : 0;
}
else if (textConcat[2] == 'Y') {
pauseY = pauseY == 0 ? 1 : 0;
}
}
//cancel movement
else if (textConcat[1] == 'C') {
if (textConcat[2] == 'X') {
destinationPositionX = currentPositionX;
}
else if (textConcat[2] == 'Y') {
destinationPositionY = currentPositionY;
}
}
else if (textConcat[1] == 'R') {
resetPosition();
}
else {
Serial.print("command not recognized...");
Serial.print(textConcat);
Serial.println();
}
textConcat = "";
}
}
void loop() {
if (Serial.available() > 0) {
processInput();
}
moveAxisXY();
}