带有伺服电机的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).