working!

nuvia.ino

@@ -1,5 +1,10 @@
+#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
@@ -16,35 +21,29 @@
 
 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;
-int newPosition = 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;
 
-void step(bool dir = false, int del = 1000){
-  if(dir) digitalWrite(DIR, HIGH);
-  if(!dir) digitalWrite(DIR, LOW);
-  
-  if(!dir) position++;
-  if(dir) position--;
-  //Serial.println(position);
-  digitalWrite(STEP, HIGH);
-  delayMicroseconds(del);
-  digitalWrite(STEP, LOW);
-  delayMicroseconds(del);
-}
+float phase = 0;
 
 void setup() {
   // put your setup code here, to run once:
   Serial.begin(9600);
 
   delay(1000);
-
-  pinMode(STEP, OUTPUT);
-  pinMode(DIR, OUTPUT);
-  pinMode(ENABLE, OUTPUT);
+  
   pinMode(REFERENCE, INPUT_PULLUP);
 
   Wire.setSDA(16);
@@ -54,6 +53,7 @@ void setup() {
   // for(int i = 0; i < 10000; i++){ // 10000 steps = 1 revolution
   //   step(true);
   // }
+
   sensor.setTimeout(500);
   if (!sensor.init())
   {
@@ -75,42 +75,85 @@ void setup() {
   setup_finished = true;
 }
 
-void setup1(){}
+void setup1(){
+  pinMode(ENABLE, OUTPUT);
 
-void loop() {
-  distance = sensor.readRangeSingleMillimeters();
-  Serial.print(distance);
-  Serial.print("\t");
-  Serial.println(position);
-  delay(10);
+  stepper.setEnablePin(1);
+  stepper.setPinsInverted(true, false, true);
+
+  stepper.setMaxSpeed(200);
+  stepper.setAcceleration(50);
 }
 
-uint32_t counter = 0;
-uint32_t last = 0;
-uint32_t next = 0;
+int counter = 0;
+
+void loop() {
+  distance = 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) {
-        digitalWrite(ENABLE, LOW);
-        analogWrite(LEDCW, 0);
-        analogWrite(LEDWW, 25);
-        step(true);
+        stepper.enableOutputs();
+        stepper.move(-100000);
       } else {
         referenced = true;
-        position = 0;
-        analogWrite(LEDCW, 50);
-        delay(100);
-        analogWrite(LEDCW, 25);
+        stepper.stop();
+        stepper.setCurrentPosition(0);
+        newPosition = random(0, maxSteps);
+        stepper.moveTo(newPosition);
       }
     } else {
-      if(position == newPosition){
-        newPosition = random(0, maxSteps);
-      } else {
-        if(position > newPosition) step(true, 1500);
-        if(position < newPosition) step(false, 1500);
+      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();
   }
 }