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

There is a [network dedicated to Arduino](https://arduino.stackexchange.com/questions) too — Pieterjan, Jul 13 '21 at 08:52
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, Jul 13 '21 at 08:57
But the motor shivers now. I had similar problem and solved it using the Thread library. — John Cho, Jul 13 '21 at 10:27

score 0 · Answer 1 · answered Jul 13 '21 at 10:28

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

It still shivers and motor doesn't move properly:( I think it's because two interval collides. But thank you! — John Cho, Jul 13 '21 at 10:44

arduino accelerometer with servo motor

1 Answers1