arduino accelerometer with servo motor - arduino

Question

I'm currently working on project to operate three motors while sending accelerometer value to computer via bluetooth module. I want the motor operation and value transmission to work seperatly. But when motors operate, the transmission stops. I used the ArduinoThread library, but since I don't understand all the usage, it still has some kind of delay.
Plus, I want two of the motors work first(just once) and the other works after specific time(also just once). But the first setup of two motors and the latter doesn't work.
Please check my code and give me some advice. Thank you!
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Servo.h>
#include <Thread.h>
const int MPU_addr=0x68; // MPU-6050 I2C address
int16_t AcX,AcY,AcZ,GyX,GyY,GyZ;
SoftwareSerial BTSerial(3,2); // TX,RX
Servo servoMini;
Servo servoUp;
Servo servoDown;
int pinMini = 6;
int pinUp = 9;
int pinDown = 10;
int i = 1;
unsigned long tPrev, tNow = 0;
Thread myThread = Thread();
void readAcc(){
Wire.endTransmission(true);
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers // 14bit??
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
//AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
//AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
//GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
//GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
//GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
BTSerial.println(AcX); // 16bit
Serial.println(AcX);
// delay(25);
}
void setup(){
// I2C
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
// serial, bluetooth
Serial.begin(9600);
BTSerial.begin(9600);
Serial.println("CLEARDATA");
Serial.println("ONLY AcX");
// servo
servoMini.attach(pinMini);
servoUp.attach(pinUp);
servoDown.attach(pinDown);
pinMode(pinMini, OUTPUT);
pinMode(pinUp, OUTPUT);
pinMode(pinDown, OUTPUT);
servoMini.write(145);
servoUp.write(180);
servoDown.write(0);
// Thread(MPU-6050)
myThread.onRun(readAcc);
myThread.setInterval(20);
}
void loop(){
while(i == 1){
//delay(1000);
tNow = millis();
myThread.run();
servoUp.attach(pinUp);
servoDown.attach(pinDown);
servoUp.write(60);
servoDown.write(120);
if((tNow - tPrev)>= 1000){
servoMini.attach(pinMini);
servoMini.write(180);
i++;
}
}
servoMini.detach();
myThread.run();
}

Answer 1

I think you probably need something like this?
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Servo.h>
#include <Thread.h>
const int MPU_addr = 0x68; // MPU-6050 I2C address
int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
SoftwareSerial BTSerial(3, 2); // TX,RX
Servo servoMini;
Servo servoUp;
Servo servoDown;
int pinMini = 6;
int pinUp = 9;
int pinDown = 10;
int i = 1;
unsigned long tPrev, tNow = 0;
Thread accelleroThread = Thread();
Thread servoThread = Thread();
void main() {
setup();
// Thread(MPU-6050)
accelleroThread.onRun(readAcc);
accelleroThread.setInterval(20);
// Thread(MPU-6050)
servoThread.onRun(controlServo);
servoThread.setInterval(20);
accelleroThread.run();
servoThread.run();
}
void setup() {
// I2C
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
// serial, bluetooth
Serial.begin(9600);
BTSerial.begin(9600);
Serial.println("CLEARDATA");
Serial.println("ONLY AcX");
// servo
servoMini.attach(pinMini);
servoUp.attach(pinUp);
servoDown.attach(pinDown);
pinMode(pinMini, OUTPUT);
pinMode(pinUp, OUTPUT);
pinMode(pinDown, OUTPUT);
servoMini.write(145);
servoUp.write(180);
servoDown.write(0);
}
void readAcc() {
while (1) {
Wire.endTransmission(true);
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers // 14bit??
AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
//AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
//AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
//GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
//GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
//GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
BTSerial.println(AcX); // 16bit
Serial.println(AcX);
// delay(25);
}
}
void controlServo() {
while (1) {
//delay(1000);
tNow = millis();
myThread.run();
servoUp.attach(pinUp);
servoDown.attach(pinDown);
servoUp.write(60);
servoDown.write(120);
if ((tNow - tPrev) >= 1000) {
servoMini.attach(pinMini);
servoMini.write(180);
i++;
}
}
servoMini.detach();
}
Note I just moved some blocks around to get 2 threads running (1 controlling the servo, 1 reading the accellero).