Braço Robótico + Arduino UNO + Controle XBOX 360 V2.0 - Tutoriais #001.2
Em 2016, durante a disciplina optativa de Eletrônica para Robótica, ministrada pelo Prof. Júlio F. Golin, atualizei o projeto do Braço Robótico, inserindo a função de armazenar posições, gerando movimentos seriados no braço robótico.
Contei com a ajuda de dois colegas de curso, a Bruna Fernanda de Oliveira e o Marcio Granzotto Rodrigues, que trabalharam comigo na programação.
O projeto inicial, que também já foi postado aqui no Blog contava com um sistema composto por uma placa Arduino UNO, um Host USB Shield onde se conectava o receptor sem fio do controle do Xbox 360, um Servo Motor Shield responsável por controlar os motores e um controle do Xbox 360.
O hardware em conjunto obedecia o controle do Xbox 360 e assim podíamos controlar os movimentos do braço manualmente. Não havia um sistema que armazenasse as posições possibilitando a movimentação repetitiva do braço. A versão 2.0 do braço traz essa função implementada, armazenando as posições em vetores.
Abaixo encontra-se a programação da Versão 2.0.
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <XBOXRECV.h>
#ifdef dobogusinclude
#include <spi4teensy3.h>
#include <SPI.h>
#endif
USB Usb;
XBOXRECV Xbox(&Usb);
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVO1MIN 150 //SERVO BASE
#define SERVO1MAX 500 //SERVO BASE
#define SERVO2MIN 150 //SERVO 1 COTOVELO
#define SERVO2MAX 400 //SERVO 1 COTOVELO
#define SERVO3MIN 150
#define SERVO3MAX 400
#define SERVO4MIN 200
#define SERVO4MAX 500
#define SERVO5MIN 150
#define SERVO5MAX 500
#define SERVO6MIN 150
#define SERVO6MAX 500
#define SERVO7MIN 350
#define SERVO7MAX 510
#define NUM_POINTS 5
// our servo # counter
uint8_t servo1 = 0; //SERVO BASE
uint8_t servo2 = 1; //SERVO 1 COTOVELO
uint8_t servo3 = 2; //SERVO 2 COTOVELO
uint8_t servo4 = 3;
uint8_t servo5 = 4;
uint8_t servo6 = 5;
uint8_t servo7 = 6;
double angle1 = SERVO1MAX/2;
double angle2 = 350; //angulo do SERVO 1 e servo 2 COTOVELO
double angle4 = SERVO4MAX/2;
double angle5 = SERVO5MAX/2;
double angle6 = SERVO6MAX/2;
double angle7 = SERVO7MAX/2;
int count = 0;
double a1[NUM_POINTS];
double a2[NUM_POINTS];
double a4[NUM_POINTS];
double a5[NUM_POINTS];
double a6[NUM_POINTS];
double a7[NUM_POINTS];
void setup() {
pwm.begin();
pwm.setPWMFreq(60); // Analog servos run at ~60 Hz updates
Serial.begin(115200);
#if !defined(__MIPSEL__)
while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
#endif
if (Usb.Init() == -1) {
Serial.print(F("\r\nOSC did not start"));
while (1); //halt
}
Serial.print(F("\r\nXbox Wireless Receiver Library Started"));
}
void loop() {
Usb.Task();
if (Xbox.XboxReceiverConnected) {
for (uint8_t i = 0; i < 4; i++) {
if (Xbox.Xbox360Connected[i]) {
if (Xbox.getButtonPress(L2, i) || Xbox.getButtonPress(R2, i)) {
Serial.print("L2: ");
Serial.print(Xbox.getButtonPress(L2, i));
Serial.print("\tR2: ");
Serial.println(Xbox.getButtonPress(R2, i));
Xbox.setRumbleOn(Xbox.getButtonPress(L2, i), Xbox.getButtonPress(R2, i), i);
}
if (Xbox.getAnalogHat(LeftHatX, i) > 7500 || Xbox.getAnalogHat(LeftHatX, i) < -7500 || Xbox.getAnalogHat(LeftHatY, i) > 7500 || Xbox.getAnalogHat(LeftHatY, i) < -7500 || Xbox.getAnalogHat(RightHatX, i) > 7500 || Xbox.getAnalogHat(RightHatX, i) < -7500 || Xbox.getAnalogHat(RightHatY, i) > 7500 || Xbox.getAnalogHat(RightHatY, i) < -7500) {
if (Xbox.getAnalogHat(LeftHatX, i) > 7500) {
if (Xbox.getAnalogHat(LeftHatX, i) == 32767){
incrementa_base();
}
Serial.print(F("LeftHatX: "));
Serial.print(Xbox.getAnalogHat(LeftHatX, i));
Serial.print("\t");
}
if (Xbox.getAnalogHat(LeftHatX, i) < -7500) {
if (Xbox.getAnalogHat(LeftHatX, i) == -32768){
decrementa_base();
}
Serial.print(F("LeftHatX: "));
Serial.print(Xbox.getAnalogHat(LeftHatX, i));
Serial.print("\t");
}
if (Xbox.getAnalogHat(LeftHatY, i) > 7500) {
if (Xbox.getAnalogHat(LeftHatY, i) == 32767){
incrementa_cotovelo();
}
Serial.print(F("LeftHatY: "));
Serial.print(Xbox.getAnalogHat(LeftHatY, i));
Serial.print("\t");
}
if (Xbox.getAnalogHat(LeftHatY, i) < -7500) {
if (Xbox.getAnalogHat(LeftHatY, i) == -32768){
decrementa_cotovelo();
}
Serial.print(F("LeftHatY: "));
Serial.print(Xbox.getAnalogHat(LeftHatY, i));
Serial.print("\t");
}
if (Xbox.getAnalogHat(RightHatX, i) > 7500) {
if (Xbox.getAnalogHat(RightHatX, i) == 32767){
incrementa_servo4();
Serial.print(F("RightHatX: "));
Serial.print(Xbox.getAnalogHat(RightHatX, i));
Serial.print("\t");
}
}
if (Xbox.getAnalogHat(RightHatX, i) < -7500) {
if (Xbox.getAnalogHat(RightHatX, i) == -32768){
decrementa_servo4();
Serial.print(F("RightHatX: "));
Serial.print(Xbox.getAnalogHat(RightHatX, i));
Serial.print("\t");
}
}
if (Xbox.getAnalogHat(RightHatY, i) > 7500) {
if (Xbox.getAnalogHat(RightHatY, i) == 32767){
decrementa_servo5();
Serial.print(F("RightHatY: "));
Serial.print(Xbox.getAnalogHat(RightHatY, i));
Serial.print("\t");
}
}
if (Xbox.getAnalogHat(RightHatY, i) < -7500) {
if (Xbox.getAnalogHat(RightHatY, i) == -32768){
incrementa_servo5();
Serial.print(F("RightHatY: "));
Serial.print(Xbox.getAnalogHat(RightHatY, i));
Serial.print("\t");
}
}
Serial.println();
}
if (Xbox.getButtonClick(UP, i)) {
Xbox.setLedOn(LED1, i);
Serial.println(F("Up"));
}
if (Xbox.getButtonClick(DOWN, i)) {
Xbox.setLedOn(LED4, i);
Serial.println(F("Down"));
}
if (Xbox.getButtonClick(LEFT, i)) {
Xbox.setLedOn(LED3, i);
Serial.println(F("Left"));
}
if (Xbox.getButtonClick(RIGHT, i)) {
Xbox.setLedOn(LED2, i);
Serial.println(F("Right"));
}
if (Xbox.getButtonClick(START, i)) {
//Xbox.setLedMode(ALTERNATING, i);
Serial.println(F("Start"));
automatico();
}
if (Xbox.getButtonClick(BACK, i)) {
//Xbox.setLedBlink(ALL, i);
Serial.println(F("Back"));
play();
}
if (Xbox.getButtonClick(L3, i))
Serial.println(F("L3"));
if (Xbox.getButtonClick(R3, i))
Serial.println(F("R3"));
if (Xbox.getButtonClick(L1, i))
Serial.println(F("L1"));
if (Xbox.getButtonClick(R1, i))
Serial.println(F("R1"));
if (Xbox.getButtonClick(XBOX, i)) {
Xbox.setLedMode(ROTATING, i);
Serial.print(F("Xbox (Battery: "));
Serial.print(Xbox.getBatteryLevel(i)); // The battery level in the range 0-3
Serial.println(F(")"));
}
if (Xbox.getButtonClick(SYNC, i)) {
Serial.println(F("Sync"));
Xbox.disconnect(i);
}
if (Xbox.getButtonClick(A, i)){
rotaciona1_mao();
Serial.println(F("A"));
}
if (Xbox.getButtonClick(B, i)){
abre_garra();
Serial.println(F("B"));
}
if (Xbox.getButtonClick(X, i)){
fecha_garra();
Serial.println(F("X"));
}
if (Xbox.getButtonClick(Y, i)){
rotaciona2_mao();
Serial.println(F("Y"));
}
}
}
}
}
void setServoPulse(uint8_t n, double pulse) {
double pulselength;
pulselength = 1000000; // 1,000,000 us per second
pulselength /= 60; // 60 Hz
Serial.print(pulselength); Serial.println(" us per period");
pulselength /= 4096; // 12 bits of resolution
Serial.print(pulselength); Serial.println(" us per bit");
pulse *= 1000;
pulse /= pulselength;
Serial.println(pulse);
pwm.setPWM(n, 0, pulse);
}
void incrementa_base(){
Serial.println(F("Incrementou na Base"));
if(angle1==SERVO1MAX){
}
else
angle1++;
Serial.print("angle: ");
Serial.print(angle1);
Serial.print("\t");
pwm.setPWM(servo1, 0, angle1);
}
void decrementa_base(){
Serial.println(F("DEcrementou na Base"));
if(angle1==SERVO1MIN){
}
else
angle1--;
Serial.print("angle: ");
Serial.print(angle1);
Serial.print("\t");
pwm.setPWM(servo1, 0, angle1);
}
void incrementa_cotovelo(){
Serial.println(F("Incrementou no Cotovelo"));
if(angle2==SERVO2MAX){
}
else
angle2++;
Serial.print("angle: ");
Serial.print(angle2);
Serial.print("\t");
pwm.setPWM(servo2, 0, angle2);
pwm.setPWM(servo3, 0, angle2);
}
void decrementa_cotovelo(){
Serial.println(F("DEcrementou no Cotovelo"));
if(angle2==SERVO2MIN){
}
else
angle2--;
Serial.print("angle: ");
Serial.print(angle2);
Serial.print("\t");
pwm.setPWM(servo2, 0, angle2);
pwm.setPWM(servo3, 0, angle2);
}
void incrementa_servo4(){
Serial.println(F("Incrementou no Servo 4"));
if(angle4==SERVO4MAX){
}
else
angle4++;
Serial.print("angle: ");
Serial.print(angle4);
Serial.print("\t");
pwm.setPWM(servo4, 0, angle4);
}
void decrementa_servo4(){
Serial.println(F("DEcrementou no Servo 4"));
if(angle4==SERVO4MIN){
}
else
angle4--;
Serial.print("angle: ");
Serial.print(angle4);
Serial.print("\t");
pwm.setPWM(servo4, 0, angle4);
}
void incrementa_servo5(){
Serial.println(F("Incrementou no Servo 5"));
if(angle5==SERVO5MAX){
}
else
angle5++;
Serial.print("angle: ");
Serial.print(angle5);
Serial.print("\t");
pwm.setPWM(servo5, 0, angle5);
}
void decrementa_servo5(){
Serial.println(F("DEcrementou no Servo 5"));
if(angle5==SERVO5MIN){
}
else
angle5--;
Serial.print("angle: ");
Serial.print(angle5);
Serial.print("\t");
pwm.setPWM(servo5, 0, angle5);
}
void fecha_garra(){
Serial.print("Fechou garra ");
angle7=SERVO7MAX;
pwm.setPWM(servo7, 0, angle7);
}
void abre_garra(){
Serial.print("abriu garra ");
angle7=SERVO7MIN;
pwm.setPWM(servo7, 0, angle7);
}
void rotaciona1_mao(){
Serial.print("girou mao 1 ");
angle6=SERVO6MIN;
pwm.setPWM(servo6, 0, angle6);
}
void rotaciona2_mao(){
Serial.print("girou mao 1 ");
angle6=SERVO6MAX;
pwm.setPWM(servo6, 0, angle6);
}
void automatico(){
if(count > NUM_POINTS - 1){
count = 0;
}
Serial.print("Salvando na posicao");
Serial.println(count);
a1[count]=angle1;
a2[count]=angle2;
a4[count]=angle4;
a5[count]=angle5;
a6[count]=angle6;
a7[count]=angle7;
count++;
}
void play(){
Serial.println("play()");
for (int i = 0; i < NUM_POINTS; i++){
Serial.print("playando");
Serial.println(i);
angle1 = a1[i];
pwm.setPWM(servo1, 0, angle1);
delay(500);
angle2 = a2[i];
pwm.setPWM(servo2, 0, angle2);
pwm.setPWM(servo3, 0, angle2);
delay(500);
angle4 = a4[i];
pwm.setPWM(servo4, 0, angle4);
delay(500);
angle5 = a5[i];
pwm.setPWM(servo5, 0, angle5);
delay(500);
angle6 = a6[i];
pwm.setPWM(servo6, 0, angle6);
delay(500);
angle7 = a7[i];
pwm.setPWM(servo7, 0, angle7);
delay(2000);
}
} 
Ola Matheus, gostei desse seu projeto e montei um igual para apresentação do meu TCC na faculdade, mas estou tendo alguns problemas com o meu robo, estou utilizando as mesmas peças, placas,etc que o seu, mas os meus servos MG995 não estão respondendo os comandos, ao ligar eles no shild pwm ele começam a girar apenas para um sentindo, e todos os 5 servos MG995 estão assim, ja os MG90S estão funcionando normal, sera que você consegue me ajudar com esse problema? obrigado
ResponderExcluirOla, vou tentar te ajudar como eu sempre faço quando encontro problemas de programação e hardware.
Excluir1. Isole o problema. Faça um programa "teste_de_motor" para acionar um único motor que automaticamente gire o motor para um lado e depois de um tempo gire o motor para o outro lado;
2. Teste individualmente cada motor nesse programa "teste_de_motor";
3. Se todos os motores funcionarem individualmente faça um programa "teste_shield_pwm", onde esse programa será a cópia do programa "teste_de_motor", porém vc vai alternar a porta PWM. Primeiro aciona a PWM1, para esquerda-direita, depois aciona a PWM2, para a esquerda-direita, e assim por diante, até testar todas as portas necessárias para o projeto.
Relate os problemas que for tendo aqui e vou tentando te ajudar. Qualquer coisa pode me mandar um e-mail também. matheus.amartim@gmail.com