160 lines
3.5 KiB
C++
160 lines
3.5 KiB
C++
#include <MovingAverage.h>
|
|
|
|
#include <Wire.h>
|
|
#include <VL53L0X.h>
|
|
#include <AccelStepper.h>
|
|
#include <MultiStepper.h>
|
|
|
|
// 20 teeth to 125 = 6.25 at 1/8 steps = 10000 steps per revolution = 251mm at 80mm diameter = ca. 36000 steps for 900mm
|
|
|
|
#define LIMITUP 2700
|
|
#define LIMITDOWN 2100
|
|
|
|
#define STEP 6
|
|
#define DIR 7
|
|
#define ENABLE 1
|
|
|
|
#define LEDWW 14
|
|
#define LEDCW 10
|
|
|
|
#define REFERENCE 18
|
|
|
|
VL53L0X sensor;
|
|
|
|
AccelStepper stepper = AccelStepper(AccelStepper::DRIVER, 6, 7);
|
|
|
|
MovingAverage filter(10);
|
|
|
|
volatile bool referenced = false;
|
|
int maxSteps = floor((LIMITUP - LIMITDOWN) * (10000/251));
|
|
volatile int position = 0;
|
|
volatile int newPosition = 0;
|
|
volatile int newSpeed = 1000;
|
|
volatile int speed = 1000;
|
|
volatile int distance = 10000;
|
|
volatile bool setup_finished = false;
|
|
volatile bool triggered = false;
|
|
volatile bool cooldown = false;
|
|
|
|
float phase = 0;
|
|
|
|
void setup() {
|
|
// put your setup code here, to run once:
|
|
Serial.begin(9600);
|
|
|
|
delay(1000);
|
|
|
|
pinMode(REFERENCE, INPUT_PULLUP);
|
|
|
|
Wire.setSDA(16);
|
|
Wire.setSCL(17);
|
|
Wire.begin();
|
|
|
|
// for(int i = 0; i < 10000; i++){ // 10000 steps = 1 revolution
|
|
// step(true);
|
|
// }
|
|
|
|
sensor.setTimeout(500);
|
|
if (!sensor.init())
|
|
{
|
|
|
|
while (1) {
|
|
Serial.println("Failed to detect and initialize sensor!");
|
|
delay(100);
|
|
}
|
|
}
|
|
|
|
sensor.setSignalRateLimit(0.1);
|
|
sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
|
|
sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
|
|
|
|
//analogWrite(LEDCW, 25);
|
|
//analogWrite(LEDWW, 25);
|
|
delay(1000);
|
|
digitalWrite(ENABLE, LOW);
|
|
setup_finished = true;
|
|
}
|
|
|
|
void setup1(){
|
|
pinMode(ENABLE, OUTPUT);
|
|
|
|
stepper.setEnablePin(1);
|
|
stepper.setPinsInverted(true, false, true);
|
|
|
|
stepper.setMaxSpeed(200);
|
|
stepper.setAcceleration(50);
|
|
}
|
|
|
|
int counter = 0;
|
|
|
|
void loop() {
|
|
distance = 10000; //filter.addSample(sensor.readRangeSingleMillimeters());
|
|
|
|
if(referenced && distance < 2000 && !triggered && !cooldown) {
|
|
triggered = true;
|
|
counter = 0;
|
|
}
|
|
|
|
if(referenced) {
|
|
analogWrite(LEDWW, sinf(phase) * 75 + 85);
|
|
phase += 0.01;
|
|
} else {
|
|
analogWrite(LEDWW, sinf(phase) * 10 + 10);
|
|
phase += 0.25;
|
|
}
|
|
|
|
Serial.print(distance);
|
|
Serial.print("\t");
|
|
Serial.println(stepper.currentPosition());
|
|
|
|
delay(5);
|
|
}
|
|
|
|
uint32_t distance_to_go = 0;
|
|
|
|
void loop1(){
|
|
if(setup_finished){
|
|
if(!referenced){
|
|
if(digitalRead(REFERENCE) == HIGH) {
|
|
stepper.enableOutputs();
|
|
stepper.move(-100000);
|
|
} else {
|
|
referenced = true;
|
|
stepper.stop();
|
|
stepper.setCurrentPosition(0);
|
|
newPosition = random(0, maxSteps);
|
|
stepper.moveTo(newPosition);
|
|
}
|
|
} else {
|
|
if(triggered && !cooldown){
|
|
stepper.setMaxSpeed(6000);
|
|
stepper.setAcceleration(5000);
|
|
stepper.moveTo(100);
|
|
distance_to_go = stepper.distanceToGo();
|
|
cooldown = true;
|
|
Serial.println("TRIGGERED");
|
|
}
|
|
if(abs(stepper.distanceToGo()) < 2){
|
|
Serial.println("DISTANCE < 2");
|
|
if(!triggered){
|
|
Serial.println("NEW POS");
|
|
newPosition = random(0, maxSteps);
|
|
stepper.setMaxSpeed((rand() % 750) + 200);
|
|
stepper.setAcceleration((rand() % 500) + 100);
|
|
stepper.moveTo(newPosition);
|
|
}
|
|
if(triggered) {
|
|
Serial.println("RESET COOLDOWN");
|
|
cooldown = false;
|
|
triggered = false;
|
|
}
|
|
}
|
|
}
|
|
if(triggered) {
|
|
analogWrite(LEDCW, map(stepper.distanceToGo(), distance_to_go, 0, 150, 0));
|
|
}
|
|
|
|
stepper.run();
|
|
}
|
|
}
|