arduino acclerometer 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){
    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 want something like Blink Without Delay, but with more tasks.

Here is my interpretation of your description which extends Blink Without Delay by adding three more tasks to:

• read the accelerometer
• perform "just once"
• perform "also just once"

I've added several Serial.print() statements to give a running commentary on what's occurring for debug purposes.

#include <Wire.h>
#include <SoftwareSerial.h>
#include <Servo.h>

SoftwareSerial BTSerial(3, 2);  // TX, RX

Servo servoMini;
Servo servoUp;
Servo servoDown;

const byte pinMini = 6;
const byte pinUp = 9;
const byte pinDown = 10;

// Timings for each event.
const unsigned long READ_ACCELEROMETER_INTERVAL = 50;
const unsigned long JUST_ONCE_INTERVAL = 1000;
const unsigned long ALSO_JUST_ONCE_INTERVAL = 2000;
const unsigned long LED_BLINK_INTERVAL = 200;

// Keep track of timestamps for each event.
unsigned long previous_blink_timestamp;
unsigned long previous_accelerometer_timestamp ;
unsigned long just_once_timestamp;
unsigned long also_just_once_timestamp;

// Flags to indicate completion of "just once" events.
bool just_once = false;
bool also_just_once = false;

// State of blinking LED.
bool led_state = false;

void setup()
{
    Serial.begin(115200);
    Serial.println("\n\nAccelerometer with Servo Test\n");

    // For blink without delay.
    pinMode(LED_BUILTIN, OUTPUT);

    // servo
    servoMini.attach(pinMini);
    servoUp.attach(pinUp);
    servoDown.attach(pinDown);

    // Perform first move.
    servoMini.write(145);
    servoUp.write(180);
    servoDown.write(0);

    // Initialise all timestamps with the same value with a single call to millis().
    previous_blink_timestamp =
    previous_accelerometer_timestamp =
    just_once_timestamp =
    also_just_once_timestamp = millis();
}

void loop()
{
    unsigned long current_timestamp = millis();

    //
    // TASK 1: Read accelerometer.
    //
    if (current_timestamp - previous_accelerometer_timestamp >= READ_ACCELEROMETER_INTERVAL)
    {
        // Read accelerometer.
        Serial.print("Reading accelerometer... ");
        readAcc();
        previous_accelerometer_timestamp += READ_ACCELEROMETER_INTERVAL;
        Serial.println("Done.");
    }

    //
    // TASK 2: Just once.
    //
    if (!just_once && current_timestamp - just_once_timestamp >= JUST_ONCE_INTERVAL)
    {
        // Perform second move.
        Serial.print("Running just once... ");
        servoUp.write(60);
        servoDown.write(120);
        just_once = true;
        Serial.println("Done.");
    }

    //
    // TASK 3: Also just once.
    //
    if (!also_just_once && current_timestamp - also_just_once_timestamp >= ALSO_JUST_ONCE_INTERVAL)
    {
        // Perform third move.
        Serial.print("Running also just once... ");
        servoMini.write(180);
        also_just_once = true;
        Serial.println("Done.");
    }

    //
    // TASK 4: Blink without delay.
    //
    if (current_timestamp - previous_blink_timestamp >= LED_BLINK_INTERVAL)
    {
        // Toggle LED state.
        led_state = !led_state;
        digitalWrite(LED_BUILTIN, led_state);
        previous_blink_timestamp += LED_BLINK_INTERVAL;
    }
}

Answer 2

Here's the final form of my project. I've added motor detach function since it still shivers a bit.

#include <SoftwareSerial.h>
#include <Servo.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;

const byte pinMini = 6;
const byte pinUp = 9;
const byte pinDown = 10;

// Timings for each event.
const unsigned long ACC_INTERVAL = 50;
const unsigned long INTERVAL = 1000;
const unsigned long INTERVAL_FIN = 2000;
const unsigned long GRIPPER_INTERVAL = 2000;
const unsigned long GRIPPER_INTERVAL_FIN= 3000;
const unsigned long LED_INTERVAL = 250;

// Keep track of timestamps for each event.
unsigned long prev_led_timestamp;
unsigned long prev_acc_timestamp;
unsigned long servo_timestamp;
unsigned long servo_timestamp_fin;
unsigned long gripper_timestamp;
unsigned long gripper_timestamp_fin;

// Flags to indicate completion of "just once" events.
bool servo = false;
bool servo_fin = false;
bool gripper = false;
bool gripper_fin = false;

// State of blinking LED.
bool led_state = false;

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

void setup(){
  //Serial.begin(9600);
  BTSerial.begin(9600);

  // For blink without delay.
  pinMode(LED_BUILTIN, OUTPUT);

  // 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)
  Wire.endTransmission(true);

  // servo
  servoMini.attach(pinMini);
  servoUp.attach(pinUp);
  servoDown.attach(pinDown);

  // Perform first move.
  servoMini.write(145);
  servoUp.write(180);
  servoDown.write(0);

  // Initialise all timestamps with the same value with a single call to millis().
  prev_led_timestamp =
  prev_acc_timestamp =
  servo_timestamp = servo_timestamp_fin =
  gripper_timestamp = gripper_timestamp_fin = millis();
}

void loop(){  
  unsigned long current_timestamp = millis();

  // TASK 1: Read acc.
  if (current_timestamp - prev_acc_timestamp >= ACC_INTERVAL){
    // Read acc.
    readAcc();
    prev_acc_timestamp += ACC_INTERVAL;
  }

  // TASK 2: Just once.
  if (!servo && current_timestamp - servo_timestamp >= INTERVAL)  {
    //Serial.print("Running just once... ");
    servoUp.attach(pinUp);
    servoDown.attach(pinDown);
    servoUp.write(60);
    servoDown.write(120);
    // retract speed control
    servo = true;
    //Serial.println("Done.");
  }
  if (!servo_fin && current_timestamp - servo_timestamp_fin >= INTERVAL_FIN){
    servoUp.detach();
    servoDown.detach();
    servo_fin = true;
  }

  // TASK 3: Also just once.
  if (!gripper && current_timestamp - gripper_timestamp >= GRIPPER_INTERVAL)  {
    // Perform third move.
    //Serial.print("Running also just once... ");
    servoMini.write(180);
    gripper = true;
    //Serial.println("Done.");
  }
  if (!gripper_fin && current_timestamp - gripper_timestamp_fin >= GRIPPER_INTERVAL_FIN){
    servoMini.detach();
    gripper_fin = true;
  }

  // TASK 4: Blink without delay.
  if (current_timestamp - prev_led_timestamp >= LED_INTERVAL){
    // Toggle LED state.
    led_state = !led_state;
    digitalWrite(LED_BUILTIN, led_state);
    prev_led_timestamp += LED_INTERVAL;
  }
}