arduino accelerometer with servo motor

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();
}

You start the readAcc method on another thread just once, which is fine. But the readAcc only does its thing once. I think you forgot to put its contents into a while (1) loop. Probably just wrap the contents of readAcc() into a while (1) loop, so that the accellerometer reading keeps on going. — Pieterjan
But the motor shivers now. I had similar problem and solved it using the Thread library. — John Cho

Pieterjan Pieterjan · Accepted Answer · 2021-07-13T10:28:47

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).

arduino accelerometer with servo motor

1 Answers