I have finished readink UKHeliBob's great article on using millis() for timing. I have used the ideas he presented to code a similar sketch for moving Servos simultaneously. The code however is not behaving like I expected. I was wondering could this be because I am using an Arduino Pro Mini. If not, I am very confused on what the problem could be. I would really appreciate your help, thanks.
#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;
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() - lastMillis > 1000) {
handleWrist();
lastMillis = millis();
}
}
@Delta_G
I am still a little confused. I created a second variable called lastMillis2 to be used in the second loop but the problem still persists.
Also the expected behaviour is that the two functions handleWrist and handleFingers run "simultaneously". The handleWrist function reads the value of a gyroscope and moves a servo accordingly. The handleFingers function reads the value of a force sensor and moves a second servo accordingly.
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();
}
}
What is happening is the handleWrist function does not seem to be executing (or at least the servo controlled by this function does not move at all), and the handleFingers function executes but not as desired. The servo motor controlled by this function rotates 90 degrees when the force sensor is pressed, however it does not respond to anything after that. The servo just remains at 90 degrees.
As written in my previous reply, the expected behaviour is that the two functions handleWrist and handleFingers run "simultaneously". The handleWrist function reads the value of a gyroscope and moves a servo accordingly. The handleFingers function reads the value of a force sensor and moves a second servo accordingly.