2

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();
} 
5
  • 1
    Your loop() function makes no sense to me. What is your intention with that code?
    – Majenko
    Jul 13 '21 at 11:49
  • To read the value from an accellerometer (I²C bus) and process/use these readings to control the servo (bluetooth, probably in opposite direction?)
    – Pieterjan
    Jul 13 '21 at 17:15
  • I think you'll probably have to look at DSP (digital signal processing). Perhaps something like this, but I haven't done this sort of things lately. You can indeed use a thread to read the values and store them in variables, another thread for dsp for delaying the signal, and a third to control the servo.
    – Pieterjan
    Jul 13 '21 at 17:24
  • @Majenko I want two motors which is servoUp and servoDown to move first just once. And after 1seconds, the other motor to move once. So I used while(i ==1). Since need to check the value form the acclerometer, so I used myThread.run() again, but I'm not sure.
    – John Cho
    Jul 14 '21 at 4:49
  • @Pieterjan The accelerometer and motor doesn't have meaningful connection. The value from sensor is sent to computer and modified for analysis seperately. I'm going to change the angle that motor moves and velocity of servomotor to check difference in acceleration.
    – John Cho
    Jul 14 '21 at 4:55
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;
    }
}
3
  • Wow! It's exactly what I wanted. I've never thought of using boolean type. Thank you very much! By the way, Can I ask you why you add the led blinking at the end?
    – John Cho
    Jul 19 '21 at 8:45
  • 1
    @JohnCho, the LED blinking is not essential, but it's useful for debugging purposes to show that the MCU hasn't frozen - known as a heartbeat signal or sign of life signal. Plus I was building upon the Blink Without Delay example to show how to add concurrent non-blocking tasks.
    – tim
    Jul 19 '21 at 16:02
  • Thank you! Now I have better understand of concurrent tasks. Sorry for late comment, by the way.
    – John Cho
    Jul 23 '21 at 8:07
1

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

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.