Ola Fazedores,
Estou fazendo um projeto de Conclusão de curso de engenharia elétrica e consiste em fazer um braço robotico programável utilizando um acelerômetro para coletar os dados.
O projeto terá somente um botao ao qual vai selecionar os modos : Operação, Programação e Run.
Modo Operação o braço irá apenas copiar os movimentos do acelerômetro que estará numa luva.
Modo Programação: O braço ira copiar os movimentos e irá guardar os dados .
Modo RUN : Irá repetir o programa gravado.
Estou fazendo o codigo de umas pesquisas que fiz, como nao entendo muito de programação me deparei com alguns problemas:
Parte do meu código é esse :
//Servo
#include <Servo.h>
#include “I2Cdev.h”
#include “MPU6050_6Axis_MotionApps20.h”
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include “Wire.h”
#endif
// se AD0 estiver em low = 0x68
// se AD0 estiver em high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- tirar comentário caso AD0 = high
#define OUTPUT_READABLE_YAWPITCHROLL
#define LED_PIN 13
bool blinkState = false;
// variáveis de controle/status do MPU
bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[100];
// Variáveis de orientação/movimento
Quaternion q; // [w, x, y, z]
VectorInt16 aa; // [x, y, z]
VectorInt16 aaReal; // [x, y, z]
VectorInt16 aaWorld; // [x, y, z]
VectorFloat gravity; // [x, y, z]
float euler[3]; // [psi, theta, phi]
float ypr[3]; // [yaw, pitch, roll]
uint8_t teapotPacket[14] = { ‘$’, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, ‘\r’, ‘\n’ };
volatile bool mpuInterrupt = false;
////////////////////////////Defines novos //////////////////////////
#define minGrip 15
#define minBase 0
#define minShou 60
#define minElbw 60
#define maxGrip 45
#define maxBase 170
#define maxShou 180
#define maxElbw 150
#define midGrip 30
#define midBase 87
#define midShou 138
#define midElbw 100
int gripPosition[100];
int basePosition[100];
int shouPosition[100];
int elbwPosition[100];
Servo myservoBase; // create servo object to control a servo yaw (Z)
Servo myservoShou; // create servo object to control a servo Roll(Y)
Servo myservoGrip; // create servo object to control a servo
Servo myservoElbw; // create servo object to control a servo Pitch(X)
int servoBaseVal; // variables to read the value from the analog pin
int servoShouVal;
int servoGripVal;
int servoElbwVal;
int servoBasePastVal; // variables to store the value from the analog pin at last loop
int servoShouPastVal;
int servoGripPastVal;
int servoElbwPastVal;
int recordPosition = 0; // if HIGH, record servo position
int execTaskCmd = 0; // if HIGH, run program
int tempo =20;
int numero = 0;
boolean botaoAtu = false;
boolean botaoAnt = false;
float rec [15];
int positionIndex = 0;
const int botao = 3;
void dmpDataReady() {
mpuInterrupt = true;
}
void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // clock de 400kHz no I2C (definir 200kHz para CPU de 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
//Define portas dos servos
myservoShou.attach(9); // Eixo Y no pino 9
myservoElbw.attach(10);// Eixo X no pino 10
myservoBase.attach(11); // Eixo Z no pino 11
pinMode(botao,INPUT);
Serial.begin(115200);
Serial.println(F(“Inicializando dispositivos I2C…”));
mpu.initialize();
// verifica conexão
Serial.println(F(“Testando conexão com os dispositivos…”));
Serial.println(mpu.testConnection() ? F(“Conectado ao MPU6050 com sucesso”) : F(“Falha ao conectar ao MPU6050”));
// carrega e configura o DMP
Serial.println(F(“Inicializando DMP…”));
devStatus = mpu.dmpInitialize();
// definidos os valores de offset
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(85);
mpu.setZAccelOffset(1788);
// garante que etapa anterior tenha terminado
if (devStatus == 0) {
// liga o DMP
Serial.println(F(“Habilitando DMP…”));
mpu.setDMPEnabled(true);
// Habilita detecção de interrupções
Serial.println(F(“Habilitando detecção de interrupção (Arduino external interrupt 0)…”));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// marca DMP como pronto para uso
Serial.println(F(“DMP pronto! Aguardando primeira interrupção…”));
dmpReady = true;
// pega tamanho de pacote esperado
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERRORS!
// 1 = erro ao carregar memoria inicial
// 2 = Configuração do DMP falhou
Serial.print(F(“Inicialização do DMP falhou (código “));
Serial.print(devStatus);
Serial.println(F(”)”));
}
// configura saída do LED
pinMode(LED_PIN, OUTPUT);
}
void loop() {
// Em caso de falha encerra o programa
if (!dmpReady) return;
// aguarda por MPU interrupt ou pacote extra disponivel
while (!mpuInterrupt && fifoCount < packetSize) {
// Espaço para programa caso Interrupt = true
}
// reseta marcador de interrupt and lê o byte INT_STATUS
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// lê a contagem de FIFO atual
fifoCount = mpu.getFIFOCount();
// checa se há overflow
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reinicia o FIFO
mpu.resetFIFO();
Serial.println(F(“FIFO overflow!”));
// checa se DMP está disponível
} else if (mpuIntStatus & 0x02) {
// aguarda entrada correta dos dados
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// le um pacote do FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// mostra angulos de Euler em graus
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
botaoAtu = digitalRead(botao);
if (botaoAtu && !botaoAnt) {
numero++;
}
switch (numero){
case 0:
Serial.print(“ypr\t”);
Serial.print(ypr[0] * 180/M_PI);
myservoBase.write(int(ypr[0] * -180/M_PI)+90); //Rotação eixo Z
Serial.print("\t");
Serial.print(ypr[1] * 180/M_PI);
myservoShou.write(int(ypr[1] * -180/M_PI)+90); // Rotação eixo Y
Serial.print("\t");
Serial.println(ypr[2] * 180/M_PI);
myservoElbw.write(int(ypr[2] * -180/M_PI)+90); // Rotação eixo X
break;
case 1:
Serial.print("GRAVANDO CODIGO");
recArmPosition();
break;
case 2:
Serial.print("Executar Programa");
Serial.println("programa gravado");
for (int i=0; i <positionIndex;i++)
{
Serial.println(basePosition[i]);
Serial.println(shouPosition[i]);
Serial.println( elbwPosition[i]);
break;
}}}
void recArmPosition(){
tempo = tempo++;
servoBaseVal = ypr[0] * 180/M_PI;
servoShouVal= ypr[1] * 180/M_PI;
servoElbwVal =ypr[2] * 180/M_PI;
switch (tempo){
case 0:
positionIndex++;
gripPosition[positionIndex] = servoGripVal;
basePosition[positionIndex] = servoBaseVal;
shouPosition[positionIndex] = servoShouVal;
elbwPosition[positionIndex] = servoElbwVal;
delay(2000);
tempo=tempo +1;
break;
case 1:
positionIndex++;
gripPosition[positionIndex] = servoGripVal;
basePosition[positionIndex] = servoBaseVal;
shouPosition[positionIndex] = servoShouVal;
elbwPosition[positionIndex] = servoElbwVal;
delay(2000);
tempo=tempo +1;
break;
case 2:
positionIndex++;
gripPosition[positionIndex] = servoGripVal;
basePosition[positionIndex] = servoBaseVal;
shouPosition[positionIndex] = servoShouVal;
elbwPosition[positionIndex] = servoElbwVal;
delay(2000);
tempo=tempo +1;
break;
case 3:
positionIndex++;
gripPosition[positionIndex] = servoGripVal;
basePosition[positionIndex] = servoBaseVal;
shouPosition[positionIndex] = servoShouVal;
elbwPosition[positionIndex] = servoElbwVal;
delay(2000);
tempo=tempo +1;
break;
case 4:
positionIndex++;
gripPosition[positionIndex] = servoGripVal;
basePosition[positionIndex] = servoBaseVal;
shouPosition[positionIndex] = servoShouVal;
elbwPosition[positionIndex] = servoElbwVal;
delay(2000);
tempo=tempo +1;
break;
case 5:
positionIndex++;
gripPosition[positionIndex] = servoGripVal;
basePosition[positionIndex] = servoBaseVal;
shouPosition[positionIndex] = servoShouVal;
elbwPosition[positionIndex] = servoElbwVal;
delay(2000);
tempo=tempo +1;
break;
case 6:
positionIndex++;
gripPosition[positionIndex] = servoGripVal;
basePosition[positionIndex] = servoBaseVal;
shouPosition[positionIndex] = servoShouVal;
elbwPosition[positionIndex] = servoElbwVal;
delay(2000);
tempo=tempo +1;
break;
case 7:
positionIndex++;
gripPosition[positionIndex] = servoGripVal;
basePosition[positionIndex] = servoBaseVal;
shouPosition[positionIndex] = servoShouVal;
elbwPosition[positionIndex] = servoElbwVal;
delay(2000);
tempo= 0;
break;
}
}
Esse é uma parte do meu código ao qual estou tentando gravar as informações do MPU nos arrays Sholder, Elbow e Base para depois reproduzir, mas esta dando erro de FIFO OVERFLOW no serial. Gostaria de ver os dados gravados no serial para saber se esta gravando mesmo. Se alguém puder me ajudar a passar desse problema eu agradeço desde já .