Arduino Pro Mini Using Millis() to Time

Projects Programming
6

The new code is below. All I did was add one more variable.

#include <Wire.h>
#include <Servo.h> 
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Servo.h"

int FSR_Pin = A0; //connected to analog 0
bool isClosed = false;
unsigned long lastMillis = 0;
unsigned long lastMillis2 = 0;

MPU6050 mpu;

int16_t ax, ay, az;
int16_t gx, gy, gz;


Servo handServo;  // create servo object to control a servo 
                // twelve servo objects00 can be created on most boards
Servo wristServo;

int pos = 0;    // variable to store the servo position 
int val;
int prevVal;

void setup(){
  Serial.begin(9600);
  handServo.attach(9);

  Wire.begin();
  Serial.begin(38400);

  Serial.println("Initialize MPU");
  mpu.initialize();
  Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
  wristServo.attach(8);
}


void handleFingers () {
  int FSRReading = analogRead(FSR_Pin);  //see whats up with the IR sensor
  Serial.println(FSRReading);                //print the value of the IR sensor

if (FSRReading > 9) {
 if (pos < 90) {
     handServo.write(pos);
      pos ++;
 }
  isClosed = true;

} else if (isClosed == true && FSRReading < 10) {
  pos = 0;
  handServo.write(pos);
  isClosed = false;
}
}

void handleWrist() {
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    val = map(ay, -17000, 17000, 0, 179);
    if (val != prevVal)
    {
        wristServo.write(val);
        prevVal = val;
    }

}

void loop(){
  if (millis() - lastMillis > 1000) {
    handleFingers ();
    lastMillis = millis();
  }
 
 if (millis() - lastMillis2 > 1000) {
    handleWrist();
    lastMillis2 = millis();
  }
}