Hey,
I am building a circuit that consists of 5 servos that move based on the input given by 2 I2C sensors (Vl53L0x) sensors. I set up the i2C channels for the sensors separately and then run the program.
Goal of the Program:
- if an object is positioned in the center of the two sensors and gets close to the sensor, the servos would rotate at a larger angle step by step.
I am experiencing quite a few issues with assembling the circuit.
Here are my issues:
-
Sometimes the two sensors don't start up even if the circuit connections and code are correct. I have to manually redo the I2C channels for both sensors and then it works. Why does this happen?
-
My servos dont move or if they do, it is not fast or they move irregularly. Why does that happen?
Here is my code:
> #include "Adafruit_VL53L0X.h"
> #include <Servo.h>
>
> Servo myservo1;
> Servo myservo_index;
> Servo myservo_middle;
> Servo myservo_ring_small;
> Servo myservo_thumb;
>
> // address we will assign if dual sensor is present
> #define LOX1_ADDRESS 0x30
> #define LOX2_ADDRESS 0x2A
>
> // set the pins to shutdown
> #define SHT_LOX1 7
> #define SHT_LOX2 6
>
> // this holds the measurement
> VL53L0X_RangingMeasurementData_t measure1;
> VL53L0X_RangingMeasurementData_t measure2;
>
> // objects for the vl53l0x
> Adafruit_VL53L0X lox1 = Adafruit_VL53L0X();
> Adafruit_VL53L0X lox2 = Adafruit_VL53L0X();
>
> //int list1[3];
> //int list2[3];
> int sensor1 = 0;
> int sensor2 = 0;
> int index = 0;
> int avg = 0;
> int pos = 0;
> int fing_pos = 30;
> int curr_pos = 90;
> int pos_finger = 60; // finger is at rest position at 60 deg (claw shape)
> unsigned long startTime_display = 0;
> unsigned long interval_display = 300;
>
>
> /*
> Reset all sensors by setting all of their XSHUT pins low for delay(10), then set all XSHUT high to bring out of reset
> Keep sensor #1 awake by keeping XSHUT pin high
> Put all other sensors into shutdown by pulling XSHUT pins low
> Initialize sensor #1 with lox.begin(new_i2c_address) Pick any number but 0x29 and it must be under 0x7F. Going with 0x30 to 0x3F is probably OK.
> Keep sensor #1 awake, and now bring sensor #2 out of reset by setting its XSHUT pin high.
> Initialize sensor #2 with lox.begin(new_i2c_address) Pick any number but 0x29 and whatever you set the first sensor to
> */
> void setID() {
> // initing LOX1
> if(!lox1.begin(LOX1_ADDRESS)) {
> // Serial.println(F("Failed to boot first VL53L0X"));
> while(1);
> }
> delay(10);
>
> // activating LOX2
> digitalWrite(SHT_LOX2, HIGH);
> delay(10);
>
> //initing LOX2
> if(!lox2.begin(LOX2_ADDRESS)) {
> // Serial.println(F("Failed to boot second VL53L0X"));
> while(1);
> }
> }
>
> void read_dual_sensors() {
> lox1.rangingTest(&measure1, false); // pass in 'true' to get debug data printout!
> lox2.rangingTest(&measure2, false); // pass in 'true' to get debug data printout!
>
> // print sensor one reading
> Serial.print(F("1_: "));
> if(measure1.RangeStatus != 4) { // if not out of range
> Serial.print(measure1.RangeMilliMeter);
> sensor1 = measure1.RangeMilliMeter;
> } else {
> // Serial.print(F("Out of range"));
> Serial.print("1000");
> }
>
> Serial.print(F(" "));
>
> // print sensor two reading
> Serial.print(F("2: "));
> if(measure2.RangeStatus != 4) {
> Serial.print(measure2.RangeMilliMeter);
> sensor2 = measure2.RangeMilliMeter;
> } else {
> // Serial.print(F("Out of range"));
> Serial.print("1000");
> }
>
> compare(sensor1, sensor2);
> Serial.println();
> }
>
> void setup() {
> Serial.begin(9600);
> myservo1.attach(9); // attaches the servo on pin 9 to the servo object
> myservo_index.attach(4); //index finger
> myservo_middle.attach(5); // middle finger
> myservo_ring_small.attach(6); //small finger and ring finger
> myservo_thumb.attach(7); // thumb
>
> finger_position(fing_pos);
>
> // wait until serial port opens for native USB devices
> while (! Serial) { delay(1); }
>
> pinMode(SHT_LOX1, OUTPUT);
> pinMode(SHT_LOX2, OUTPUT);
>
> Serial.println(F("Shutdown pins inited..."));
>
> digitalWrite(SHT_LOX1, LOW);
> digitalWrite(SHT_LOX2, LOW);
>
> Serial.println(F("Both in reset mode...(pins are low)"));
>
>
> Serial.println(F("Starting..."));
> setID();
>
> }
>
> void finger_position(int a){
> myservo_index.write(a); //index finger
> myservo_middle.write(a); // middle finger
> myservo_ring_small.write(a); //small finger and ring finger
> myservo_thumb.write(a); // thumb
> }
>
> void open_close(int a, int b){
> int s1 = a;
> int s2 = b;
> Serial.println("WERE ARE IN");
> if (122 < s1 && s1 < 142){
> int pos_f = 60;
> Serial.println("60");
> finger_position(pos_f);
> }
> if (110 < s1 && s1 <= 122){
> int pos_f = 80;
> Serial.println("80");
> finger_position(pos_f);
> }
> if (100 < s1 && s1 <= 110){
> int pos_f = 90;
> Serial.println("90");
> finger_position(pos_f);
> }
>
> if (90 < s1 && s1 <= 100){
> int pos_f = 100;
> Serial.println("100");
> finger_position(pos_f);
> }
>
> if (80 < s1 && s1 <= 90){
> int pos_f = 110;
> Serial.println("110");
> finger_position(pos_f);
> }
>
> if (70 < s1 && s1 <= 80){
> int pos_f = 120;
> Serial.println("120");
> finger_position(pos_f);
> }
>
> if (60 < s1 && s1 <= 70){
> int pos_f = 140;
> Serial.println("140");
> finger_position(pos_f);
> }
>
> if (50 < s1 && s1 <= 60){
> int pos_f = 160;
> Serial.println("160");
> finger_position(pos_f);
> }
> }
>
> void compare(int a, int b){
> // unsigned compare_currentTime = millis();
> int diff = abs(a - b);
> int sen1 = a;
> int sen2 = b;
> if (a > b){
> if (diff > 30){
> // A: Sensor 1, B: Sensor 2
> Serial.println("Sensor 1 is higher");
> }
> if (diff <= 30){
> Serial.println("Within bounds");
> open_close(a, b);
> }
> }
> else if (b > a){
> if (diff > 30){
> Serial.println("Sensor 2 takes lead");
> }
> if (diff <= 30){
> Serial.println("Within bound");
> open_close(a, b);
> }
> }
> }
>
> void loop() {
> unsigned currentTime = millis();
> if (currentTime - startTime_display >= interval_display){
> startTime_display = currentTime;
> read_dual_sensors();
> // compare(sensor1, sensor2);
> }
>
>
> }
>
Ive attached is my circuit diagram to this question. Could you please analyze it and tell me what I'm doing wrong and how I could make my servos move faster?
Schematic_circuit_2022-07-01.pdf - Google Drive
Thank you!
