175 lines
4.1 KiB
C++
175 lines
4.1 KiB
C++
|
|
|
|
/*
|
|
Example sketch to control a stepper motor with A4988 stepper motor driver,
|
|
AccelStepper library and Arduino: continuous rotation.
|
|
More info: https://www.makerguides.com
|
|
*/
|
|
|
|
// Include the AccelStepper library:
|
|
#include "AccelStepper.h"
|
|
|
|
#include <Wire.h>
|
|
#include <VL53L0X.h>
|
|
|
|
// Define stepper motor connections and motor interface type.
|
|
#define dirPin 2
|
|
#define stepPin 3
|
|
#define motorInterfaceType 1
|
|
// Motor interface type must be set to 1 when using a driver
|
|
#define ms1Pin 4
|
|
#define ms2Pin 5
|
|
#define ms3Pin 6
|
|
|
|
#define speedPin A0
|
|
double lastTime = 0;
|
|
// double lastTimesensors = 0;
|
|
double thisTime = 0;
|
|
const int SPEED_MIN = 50;
|
|
const int SPEED_MAX = 800;
|
|
|
|
|
|
// Create a new instance of the AccelStepper class:
|
|
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
|
|
|
|
// IR SENSORS
|
|
|
|
#define XSHUT_pin4 8 //not required for address change
|
|
#define XSHUT_pin3 9
|
|
#define XSHUT_pin2 10
|
|
#define XSHUT_pin1 11
|
|
|
|
//ADDRESS_DEFAULT 0b0101001 or 41
|
|
#define Sensor4_newAddress 41 //not required address change
|
|
#define Sensor3_newAddress 42
|
|
#define Sensor2_newAddress 43
|
|
#define Sensor1_newAddress 44
|
|
|
|
// IR sensors
|
|
VL53L0X sensor4;
|
|
VL53L0X sensor3;
|
|
VL53L0X sensor2;
|
|
VL53L0X sensor1;
|
|
|
|
|
|
// bool last_sensor1_state = 0;
|
|
// bool last_sensor2_state = 0;
|
|
// bool last_sensor3_state = 0;
|
|
// bool last_sensor4_state = 0;
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
// Set the maximum speed in steps per second:
|
|
stepper.setMaxSpeed(4000);
|
|
|
|
// MS
|
|
// 1 2 3
|
|
// 0 0 0 full
|
|
// 1 0 0 1/2
|
|
// 0 1 0 1/4
|
|
// 0 0 1 1/8
|
|
// 1 1 1 1/16
|
|
digitalWrite(ms1Pin, 1);
|
|
digitalWrite(ms2Pin, 1);
|
|
digitalWrite(ms3Pin, 1);
|
|
|
|
// Set the speed in steps per second:
|
|
stepper.setSpeed(200);
|
|
|
|
|
|
// VL53L0X
|
|
|
|
/*WARNING*/
|
|
//Shutdown pins of VL53L0X ACTIVE-LOW-ONLY NO TOLERANT TO 5V will fry them
|
|
pinMode(XSHUT_pin1, OUTPUT);
|
|
pinMode(XSHUT_pin2, OUTPUT);
|
|
pinMode(XSHUT_pin3, OUTPUT);
|
|
pinMode(XSHUT_pin4, OUTPUT);
|
|
|
|
digitalWrite(XSHUT_pin1, LOW);
|
|
digitalWrite(XSHUT_pin2, LOW);
|
|
digitalWrite(XSHUT_pin3, LOW);
|
|
digitalWrite(XSHUT_pin4, LOW);
|
|
|
|
Wire.begin();
|
|
|
|
//Change address of sensor and power up next one
|
|
|
|
pinMode(XSHUT_pin1, INPUT);
|
|
sensor1.init();
|
|
sensor1.setAddress(Sensor1_newAddress);
|
|
delay(10);
|
|
|
|
pinMode(XSHUT_pin2, INPUT);
|
|
sensor2.init();
|
|
sensor2.setAddress(Sensor2_newAddress);
|
|
delay(10);
|
|
|
|
pinMode(XSHUT_pin3, INPUT);
|
|
sensor3.init();
|
|
sensor3.setAddress(Sensor3_newAddress);
|
|
delay(10);
|
|
|
|
pinMode(XSHUT_pin4, INPUT);
|
|
sensor4.init();
|
|
sensor4.setAddress(Sensor4_newAddress);
|
|
delay(10);
|
|
|
|
|
|
sensor1.setTimeout(0);
|
|
sensor2.setTimeout(0);
|
|
sensor3.setTimeout(0);
|
|
sensor4.setTimeout(0);
|
|
|
|
// // if (!sensor.init())
|
|
// // {
|
|
// // Serial.println("Failed to detect and initialize sensor!");
|
|
// // while (1) {}
|
|
// // }
|
|
sensor1.startContinuous();
|
|
sensor2.startContinuous();
|
|
sensor3.startContinuous();
|
|
sensor4.startContinuous();
|
|
}
|
|
|
|
void loop() {
|
|
thisTime = millis();
|
|
if ((thisTime - lastTime) > 1000) {
|
|
lastTime = thisTime;
|
|
int speed_in = analogRead(speedPin);
|
|
Serial.print("speed_in : ");
|
|
Serial.print(speed_in);
|
|
int speed = map(speed_in, 0, 1023, SPEED_MIN, SPEED_MAX);
|
|
Serial.print(" | speed : ");
|
|
Serial.println(speed);
|
|
stepper.setSpeed(speed);
|
|
}
|
|
|
|
// Step the motor with a constant speed as set by setSpeed():
|
|
stepper.runSpeed();
|
|
|
|
int state1 = sensor1.readRangeContinuousMillimeters();
|
|
Serial.print("s1 : ");
|
|
Serial.print(state1);
|
|
Serial.print(" | ");
|
|
|
|
int state2 = sensor2.readRangeContinuousMillimeters();
|
|
Serial.print("s2 : ");
|
|
Serial.print(state2);
|
|
Serial.print(" | ");
|
|
|
|
int state3 = sensor3.readRangeContinuousMillimeters();
|
|
Serial.print("s3 : ");
|
|
Serial.print(state3);
|
|
Serial.print(" | ");
|
|
|
|
int state4 = sensor4.readRangeContinuousMillimeters();
|
|
Serial.print("s4 : ");
|
|
Serial.println(state4);
|
|
|
|
|
|
// if (sensor1.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
|
|
// if (sensor2.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
|
|
// if (sensor3.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
|
|
// if (sensor4.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
|
|
} |