85 lines
2.0 KiB
C++
85 lines
2.0 KiB
C++
#include "StepperController.h"
|
|
#include "StepperXController.h"
|
|
#include "Time.h"
|
|
#include <Arduino_FreeRTOS.h>
|
|
|
|
#define JoystickPin 63
|
|
|
|
#define grabPin 19
|
|
|
|
Time time;
|
|
|
|
void setup(){
|
|
// Start serial communication at 115200 baud
|
|
Serial.begin(115200);
|
|
|
|
// Set the pin mode for the defined pins
|
|
pinMode(grabPin, INPUT_PULLUP);
|
|
|
|
initSteppers();
|
|
initXStepper();
|
|
}
|
|
|
|
bool isGrabbing = false;
|
|
float startGrabbingTime = 0;
|
|
const float moveToDropZoneTime = 1750; // How long to move right (relative to me)
|
|
|
|
const int joyCriticalPoint = 10;
|
|
|
|
void loop(){
|
|
// Updates deltatime
|
|
time.update();
|
|
|
|
stepperXLoop();
|
|
|
|
// Joystick. TODO: Make this only run when needed. Too lazy now.
|
|
int joystickValueRaw = analogRead(JoystickPin);
|
|
int joystickValue = map(joystickValueRaw, 0, 1023, 0, 100);
|
|
bool left = joystickValue > 100-joyCriticalPoint;
|
|
bool right = joystickValue < joyCriticalPoint;
|
|
|
|
// Serial.print("Left: ");
|
|
// Serial.print(left);
|
|
// Serial.print(" Right: ");
|
|
// Serial.println(right);
|
|
|
|
if (isGrabbing && millis() > startGrabbingTime + moveToDropZoneTime) {
|
|
isGrabbing = false;
|
|
processJoystickInput(false, false); // Quickly stop motor
|
|
delay(1000);
|
|
moveStepper(StepperAxis::Head, 10000, 100); // open hand
|
|
}
|
|
|
|
processJoystickInput(left, right);
|
|
|
|
if (digitalRead(grabPin) == LOW){
|
|
beginGrabSequence();
|
|
//initGrabber();
|
|
}
|
|
|
|
// //Serial.print(millis());
|
|
|
|
// Serial.print("Left state: ");
|
|
// Serial.print(left);
|
|
// Serial.print(" | ");
|
|
// Serial.print("Right state: ");
|
|
// Serial.println(right);
|
|
|
|
|
|
delay(10); // delay in milliseconds
|
|
}
|
|
|
|
void initGrabber(){
|
|
moveStepper(StepperAxis::Head, 100000, 1000);
|
|
}
|
|
|
|
void beginGrabSequence(){
|
|
moveStepper(StepperAxis::Head, 10000, 100); // open hand
|
|
moveStepper(StepperAxis::Y, 10000, -1100);
|
|
moveStepper(StepperAxis::Head, 1000, -100, 100); // close hand
|
|
moveStepper(StepperAxis::Y, 10000, 1100);
|
|
|
|
isGrabbing = true;
|
|
startGrabbingTime = millis();
|
|
}
|