BOTAlex
/
ClawMachineCosplay
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: BOTAlex:Much-more-simplified
Branches Tags
BOTAlex:master
BOTAlex:Much-more-simplified
BOTAlex:testing-rtos-controlling-steppers
..
compare: BOTAlex:master
Branches Tags
BOTAlex:Much-more-simplified
BOTAlex:testing-rtos-controlling-steppers
BOTAlex:master

No commits in common. "Much-more-simplified" and "master" have entirely different histories.
Much-more- ... master

14 changed files with 209 additions and 525 deletions
Show Stats Expand all files Collapse all files

57
.vscode/settings.json vendored
View File

@ -1,57 +0,0 @@
{
"files.associations": {
"*.js": "javascript",
"*.h": "cpp",
"string": "cpp",
"vector": "cpp",
"new": "cpp",
"algorithm": "cpp",
"associative_base": "cpp",
"basic_definitions": "cpp",
"bitset": "cpp",
"cctype": "cpp",
"char_traits": "cpp",
"cmath": "cpp",
"complex": "cpp",
"cstddef": "cpp",
"cstdint": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"deque": "cpp",
"exception": "cpp",
"func_exception": "cpp",
"functional": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"ios": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"istream_helpers": "cpp",
"iterator": "cpp",
"iterator_base": "cpp",
"limits": "cpp",
"list": "cpp",
"locale": "cpp",
"map": "cpp",
"memory": "cpp",
"numeric": "cpp",
"ostream": "cpp",
"ostream_helpers": "cpp",
"queue": "cpp",
"serstream": "cpp",
"set": "cpp",
"sstream": "cpp",
"stack": "cpp",
"stdexcept": "cpp",
"streambuf": "cpp",
"string_iostream": "cpp",
"type_traits": "cpp",
"typeinfo": "cpp",
"utility": "cpp",
"valarray": "cpp"
}
}

22
platformio.ini
View File

@ -16,25 +16,3 @@ framework = arduino
lib_extra_dirs = ~/Documents/Arduino/libraries lib_extra_dirs = ~/Documents/Arduino/libraries
lib_deps = lib_deps =
FreeRTOS FreeRTOS
upload_speed = 115200
[env:esp32-pico]
platform = espressif32
board = esp32dev
framework = arduino
; lib_extra_dirs = ~/Documents/Arduino/libraries
lib_deps =
Arduino
vector
upload_speed = 921600
monitor_speed = 115200
build_type = debug
build_flags =
-Og
-g3
-ggdb
-DCORE_DEBUG_LEVEL=5
-std=gnu++17
debug_tool = esp-prog
debug_init_break = tbreak setup
upload_protocol = esptool

144
src/ClawMachineOverhaul.ino
View File

@ -1,100 +1,84 @@
#include <Arduino.h>
#ifndef ESP32
#include <Arduino_FreeRTOS.h>
#endif
#include "StepperController.h" #include "StepperController.h"
#include "ZCodeParser.h" #include "StepperXController.h"
#include "Shared/MotorMoveParams.h" #include "Time.h"
#include "Shared/Steppers.h" #include <Arduino_FreeRTOS.h>
#include "InputController.h"
// [X, Y, Head] #define JoystickPin 63
#define NUM_STEPPERS 3
Stepper steppers[NUM_STEPPERS] = {
{38, 55, 54, 0, 'x', false, false, true},
{56, 61, 60, 0, 'y', false, false, true},
{30, 34, 36, 0, 'e', false, false, true},
};
#pragma region Not gonna mess with this magic #define grabPin 19
#ifdef ESP32 Time time;
// Timer Handles
hw_timer_t *timer1 = NULL;
hw_timer_t *timer2 = NULL;
hw_timer_t *timer3 = NULL;
// Interrupt Service Routines for each timer void setup(){
void IRAM_ATTR timer1ISR() // Start serial communication at 115200 baud
{ Serial.begin(115200);
if (motorsEnabled[0])
{ // Set the pin mode for the defined pins
digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0])); pinMode(grabPin, INPUT_PULLUP);
}
initSteppers();
initXStepper();
} }
void IRAM_ATTR timer2ISR() bool isGrabbing = false;
{ float startGrabbingTime = 0;
if (motorsEnabled[1]) const float moveToDropZoneTime = 1750; // How long to move right (relative to me)
{
digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1]));
}
}
void IRAM_ATTR timer3ISR() const int joyCriticalPoint = 10;
{
if (motorsEnabled[2])
{
digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2]));
}
}
#else
// Timers for stepping the motors void loop(){
ISR(TIMER3_COMPA_vect) { // Updates deltatime
if (steppers[0].steppingEnabled && steppers[0].moveQueue != 0) { time.update();
digitalWrite(steppers[0].stepPin, !digitalRead(steppers[0].stepPin));
}
}
ISR(TIMER4_COMPA_vect) {
if (steppers[1].steppingEnabled && steppers[1].moveQueue != 0) {
digitalWrite(steppers[1].stepPin, !digitalRead(steppers[1].stepPin));
}
}
ISR(TIMER5_COMPA_vect) {
if (steppers[2].steppingEnabled && steppers[2].moveQueue != 0) {
digitalWrite(steppers[2].stepPin, !digitalRead(steppers[2].stepPin));
}
}
#endif
#pragma endregion stepperXLoop();
void anotherFunc(void * params){ // 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;
while (true) // Serial.print("Left: ");
{ // Serial.print(left);
StepperController::SetMotorEnabled('x', true); // Serial.print(" Right: ");
StepperController::SetMovementEnabled('x', true); // Serial.println(right);
StepperController::QueueMove('x', -2000);
Serial.println("Added to queue"); if (isGrabbing && millis() > startGrabbingTime + moveToDropZoneTime) {
vTaskDelay(pdMS_TO_TICKS(5000)); isGrabbing = false;
processJoystickInput(false, false); // Quickly stop motor
delay(1000);
moveStepper(StepperAxis::Head, 10000, 100); // open hand
} }
processJoystickInput(left, right);
vTaskDelete(NULL); 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 setup() { void initGrabber(){
Serial.begin(115200); moveStepper(StepperAxis::Head, 100000, 1000);
StepperController::Init(steppers, NUM_STEPPERS);
xTaskCreate(StepperController::MotorTask, "Motors", 1024, NULL, 5, NULL);
// xTaskCreate(anotherFunc, "Other loop", 1024, NULL, 5, NULL);
xTaskCreate(InputControlTask, "Input", 1024, NULL, 5, NULL);
} }
void loop() { void beginGrabSequence(){
// The RTOS scheduler manages the tasks. 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();
} }

56
src/InputController.h
View File

@ -1,56 +0,0 @@
#include <Arduino.h>
#ifndef ESP32
#include <Arduino_FreeRTOS.h>
#endif
#include "StepperController.h"
#define JOYSTICK_PIN 63
#define JOY_CRITICAL_POINT 25
#define BTN_PIN 19
TaskHandle_t inputTask;
void ClawProtocol(void *params) {
Serial.println("Started button protocol");
StepperController::SetMotorEnabled('y', true);
StepperController::SetMovementEnabled('y', true);
StepperController::SetMoveQueue('y', 3000);
vTaskResume(inputTask);
Serial.println("Button protocol done");
vTaskDelete(NULL);
}
void InputControlTask(void *params) {
inputTask = xTaskGetCurrentTaskHandle();
pinMode(BTN_PIN, INPUT_PULLUP); // Setup button pin
while (true) {
int joystickValueRaw = analogRead(JOYSTICK_PIN);
int joystickValue = map(joystickValueRaw, 0, 1023, 0, 100);
bool leftMovement = (joystickValue > (100 - JOY_CRITICAL_POINT));
bool rightMovement = (joystickValue < JOY_CRITICAL_POINT);
bool isButtonPress = digitalRead(BTN_PIN) == LOW;
if (isButtonPress){
xTaskCreate(ClawProtocol, "Button protocol", 500, (void*)&inputTask, 100, NULL);
vTaskSuspend(inputTask);
}
if (leftMovement) {
StepperController::SetMotorEnabled('x', true);
StepperController::SetMovementEnabled('x', true);
StepperController::SetMoveQueue('x', 150);
}
if (rightMovement) {
StepperController::SetMotorEnabled('x', true);
StepperController::SetMovementEnabled('x', true);
StepperController::SetMoveQueue('x', -150);
}
vTaskDelay(pdMS_TO_TICKS(100)); // Poll every x
}
}

9
src/Shared/MotorMoveParams.h
View File

@ -1,9 +0,0 @@
#ifndef MOTOR_MOVE_PARAMS_H
#define MOTOR_MOVE_PARAMS_H
typedef struct {
char motorChar;
float moveTimeMs;
} MotorMoveParams;
#endif

16
src/Shared/Steppers.h
View File

@ -1,16 +0,0 @@
#ifndef STEPPER_H
#define STEPPER_H
#include <Arduino.h>
struct Stepper {
uint8_t enPin;
uint8_t dirPin;
uint8_t stepPin;
int moveQueue;
char axis;
bool steppingEnabled;
bool motorEnabled;
bool disableOnIdle;
};
#endif

12
src/Shared/ZCommand.h
View File

@ -1,12 +0,0 @@
#ifndef ZCOMMAND_H
#define ZCOMMAND_H
#include <Arduino.h>
#include <vector>
typedef struct
{
String command;
std::vector<String> params;
} ZCommand;
#endif

141
src/StepperController.cpp
View File

@ -1,141 +0,0 @@
#include "StepperController.h"
// Static variable definitions
int StepperController::NumOfSteppers = 0;
Stepper *StepperController::Steppers = nullptr;
long StepperController::lastUpdateTime = 0;
int StepperController::getStepperIndex(const char axis)
{
for (size_t i = 0; i < StepperController::NumOfSteppers; i++)
{
if (StepperController::Steppers[i].axis == axis)
{
// Serial.print("Found motor: ");
// Serial.println(i);
return i;
}
}
return -1;
}
void StepperController::Init(Stepper *steppers, int numOfSteppers)
{
StepperController::Steppers = steppers;
StepperController::NumOfSteppers = numOfSteppers;
StepperController::lastUpdateTime = 0;
for (size_t i = 0; i < NumOfSteppers; i++)
{
pinMode(steppers[i].enPin, OUTPUT);
pinMode(steppers[i].dirPin, OUTPUT);
pinMode(steppers[i].stepPin, OUTPUT);
}
#ifdef ESP32
// Timer 1 - Equivalent to TIMER1_COMPA_vect
timer1 = timerBegin(0, 80, true); // Timer 0, prescaler 80 (1us per tick)
timerAttachInterrupt(timer1, &timer1ISR, true);
timerAlarmWrite(timer1, 500, true); // 500us interval
timerAlarmEnable(timer1);
// Timer 2 - Equivalent to TIMER3_COMPA_vect
timer2 = timerBegin(1, 80, true); // Timer 1, prescaler 80 (1us per tick)
timerAttachInterrupt(timer2, &timer2ISR, true);
timerAlarmWrite(timer2, 500, true); // 500us interval
timerAlarmEnable(timer2);
// Timer 3 - Equivalent to TIMER4_COMPA_vect
timer3 = timerBegin(2, 80, true); // Timer 2, prescaler 80 (1us per tick)
timerAttachInterrupt(timer3, &timer3ISR, true);
timerAlarmWrite(timer3, 500, true); // 500us interval
timerAlarmEnable(timer3);
#else
cli();
// Timer 3 (16-bit)
TCCR3A = 0;
TCCR3B = (1 << WGM32) | (1 << CS30); // CTC mode, no prescaler
OCR3A = 499; // Compare match value
TIMSK3 |= (1 << OCIE3A); // Enable Timer1 Compare A Match Interrupt
// Timer 4 (16-bit)
TCCR4A = 0;
TCCR4B = (1 << WGM42) | (1 << CS40); // CTC mode, no prescaler
OCR4A = 499; // Compare match value
TIMSK4 |= (1 << OCIE4A); // Enable Timer3 Compare A Match Interrupt
// Timer 5 (16-bit)
TCCR5A = 0;
TCCR5B = (1 << WGM52) | (1 << CS50); // CTC mode, no prescaler
OCR5A = 499; // Compare match value
TIMSK5 |= (1 << OCIE5A); // Enable Timer1 Compare A Match Interrupt
sei();
#endif
}
void StepperController::MotorTask(void *params)
{
while (true)
{
// Serial.println("Motor loop");
long sinceLastRunTime = millis() - StepperController::lastUpdateTime;
for (size_t i = 0; i < (size_t)StepperController::NumOfSteppers; i++)
{
Stepper *stepper = &StepperController:: Steppers[i];
bool isNegative = signbit(stepper->moveQueue);
if (stepper->steppingEnabled && stepper->moveQueue != 0)
{
if (stepper->moveQueue > 0) {
stepper->moveQueue -= std::min((int)sinceLastRunTime, abs(stepper->moveQueue));
} else {
stepper->moveQueue += std::min((int)sinceLastRunTime, abs(stepper->moveQueue));
}
Serial.println(stepper->moveQueue);
digitalWrite(stepper->dirPin, isNegative);
if (stepper->disableOnIdle && stepper->moveQueue == 0){
stepper->motorEnabled = false;
}
}
// Update motor enable states
digitalWrite(stepper->enPin, !stepper->motorEnabled);
}
StepperController::lastUpdateTime = millis();
vTaskDelay(pdMS_TO_TICKS(50));
}
}
void StepperController::SetMovementEnabled(const char axis, bool enabled)
{
int index = getStepperIndex(axis);
StepperController::Steppers[index].steppingEnabled = enabled;
}
void StepperController::SetMoveQueue(const char axis, int moveTimeMs)
{
int index = getStepperIndex(axis);
StepperController::Steppers[index].moveQueue = moveTimeMs;
}
void StepperController::QueueMove(const char axis, int moveTimeMs)
{
int index = getStepperIndex(axis);
StepperController::Steppers[index].moveQueue += moveTimeMs;
}
void StepperController::ClearMoveQueue(const char axis)
{
int index = getStepperIndex(axis);
StepperController::Steppers[index].moveQueue = 0;
}
void StepperController::SetMotorEnabled(const char axis, bool enabled)
{
int index = getStepperIndex(axis);
StepperController::Steppers[index].motorEnabled = enabled;
}

101
src/StepperController.h
View File

@ -1,37 +1,76 @@
#ifndef STEPPER_CONTROLLER_H #include <SpeedyStepper.h>
#define STEPPER_CONTROLLER_H
#include <Arduino.h> #define NUM_STEPPERS 3
#ifndef ESP32 // [X, Y, Head]
#include <Arduino_FreeRTOS.h> const int stepPins[NUM_STEPPERS] = {60, 36};
#endif const int dirPins[NUM_STEPPERS] = {61, 34};
#include <Shared/ZCommand.h> const int enablePins[NUM_STEPPERS] = {56, 30};
#include <Shared/MotorMoveParams.h>
#include <Shared/Steppers.h>
#include <algorithm>
class StepperController
{
private:
static int NumOfSteppers;
static Stepper *Steppers;
static long lastUpdateTime;
static int getStepperIndex(const char axis); enum StepperAxis{
Y,
public: Head
static void Init(Stepper *steppers, int numOfSteppers = 3);
static void MotorTask(void * params);
static void SetMovementEnabled(const char axis, bool enabled);
static void SetMoveQueue(const char axis, const int moveTimeMs);
static void QueueMove(const char axis, const int moveTimeMs);
static void ClearMoveQueue(const char axis);
static void SetMotorEnabled(const char axis, bool enabled);
}; };
SpeedyStepper steppers[NUM_STEPPERS];
bool isMoving[NUM_STEPPERS];
#endif // void stepperLoop(){
// for (int i = 0; i < NUM_STEPPERS; i++) {
// SpeedyStepper* selectedStepper = &(steppers[i]);
// if (!selectedStepper->motionComplete())
// {
// selectedStepper->processMovement();
// }
// }
// }
void moveStepperByIndex(int stepperIndex, int speed, int dist, int accel = 2000){
isMoving[stepperIndex] = true;
SpeedyStepper* selectedStepper = &(steppers[stepperIndex]);
selectedStepper->setAccelerationInMillimetersPerSecondPerSecond(accel);
selectedStepper->setSpeedInStepsPerSecond(speed);
selectedStepper->setupRelativeMoveInMillimeters(dist);
while (!selectedStepper->motionComplete())
{
selectedStepper->processMovement();
}
}
void moveStepper(StepperAxis axis, int speed, int dist, int accel = 2000) {moveStepperByIndex(axis, speed, dist, accel);}
void stopStepperByIndex(int stepperIndex){
if (!isMoving[stepperIndex]) return;
if (!steppers[stepperIndex].motionComplete()) return;
steppers[stepperIndex].setupStop();
isMoving[stepperIndex] = false;
}
void stopStepper(StepperAxis axis){stopStepperByIndex(axis);}
void initSteppers(){
for (int i = 0; i < NUM_STEPPERS; i++) {
pinMode(stepPins[i], OUTPUT);
pinMode(dirPins[i], OUTPUT);
pinMode(enablePins[i], OUTPUT);
// Enable the stepper motor
digitalWrite(enablePins[i], HIGH); // Assuming HIGH disables the motor
}
for (int i = 0; i < NUM_STEPPERS; i++) {
digitalWrite(enablePins[i], LOW); // Assuming LOW enables the motor
// Attach the stepper motor to the pins
steppers[i].connectToPins(stepPins[i], dirPins[i]);
steppers[i].setStepsPerRevolution(200);
}
moveStepper(StepperAxis::Y, 10000, 1000);
moveStepper(StepperAxis::Head, 10000, 100);
}

34
src/StepperXController.h Normal file
View File

@ -0,0 +1,34 @@
#include <ContinuousStepper.h>
#include <ContinuousStepper/Tickers/Tone.hpp>
ContinuousStepper<StepperDriver, ToneTicker> stepper;
void initXStepper(){
stepper.begin(/*step=*/54, /*dir=*/55);
stepper.setAcceleration(10000000);
pinMode(38, OUTPUT);
}
void stepperXLoop(){
stepper.loop();
}
bool isMovingLeft = false;
void processJoystickInput(bool left, bool right){
if (left){
// if (!isMovingLeft){
// isMovingLeft = true;
// stepper.stop();
// }
stepper.spin(14000);
}
else if (right){
// if (isMovingLeft){
// isMovingLeft = false;
// stepper.stop();
// }
stepper.spin(-14000);
}
else
stepper.stop();
}

21
src/Time.h Normal file
View File

@ -0,0 +1,21 @@
class Time {
private:
unsigned long lastTime;
float m_deltaTime;
public:
Time() {
lastTime = millis();
m_deltaTime = 0.0f;
}
void update() {
unsigned long currentTime = millis();
m_deltaTime = (currentTime - lastTime) / 1000.0f; // Convert to seconds
lastTime = currentTime;
}
float deltaTime() {
return m_deltaTime;
}
};

94
src/ZCodeParser.h
View File

@ -1,94 +0,0 @@
#include <Arduino.h>
#include <vector>
#include <Shared/ZCommand.h>
class ZCodeParser
{
private:
static void splitString(const char *input, char delimiter, std::vector<String> &result);
public:
static std::vector<ZCommand> ParseString(const String &inString);
};
void ZCodeParser::splitString(const char *input, char delimiter, std::vector<String> &result)
{
// Serial.println("Splitting String: ");
result.clear();
size_t start = 0;
size_t len = strlen(input);
for (size_t i = 0; i <= len; i++)
{
if (input[i] == delimiter || input[i] == '\0')
{
if (i > start)
{
result.push_back(String(input + start).substring(0, i - start)); // Fix applied here
}
start = i + 1;
}
}
// Serial.println("Split Result:");
// for (const auto &s : result)
// {
// Serial.print("[" + s + "] ");
// }
// Serial.println();
}
std::vector<ZCommand> ZCodeParser::ParseString(const String &inString)
{
// Serial.println("\n--- Parsing Started ---");
// Serial.println("Heap Before Parsing: " + String(ESP.getFreeHeap()));
std::vector<String> stringCommands;
splitString(inString.c_str(), '\n', stringCommands);
// Serial.println("After Splitting into Commands:");
// for (size_t i = 0; i < stringCommands.size(); i++)
// {
// Serial.println("Command[" + String(i) + "]: " + stringCommands[i]);
// }
// Remove comments
for (String &command : stringCommands)
{
int commentIndex = command.indexOf(';');
if (commentIndex != -1)
{
command = command.substring(0, commentIndex);
// Serial.println("Comment Removed: " + command);
}
}
std::vector<ZCommand> commands;
for (const String &commandLine : stringCommands)
{
std::vector<String> separatedStrings;
splitString(commandLine.c_str(), ' ', separatedStrings);
if (separatedStrings.empty())
continue;
ZCommand command;
command.command = separatedStrings[0];
// Serial.println("Processing Command: " + command.command);
for (size_t j = 1; j < separatedStrings.size(); j++)
{
command.params.push_back(separatedStrings[j]);
// Serial.println("Added Param: " + command.params.back());
}
commands.push_back(command);
// Serial.println("Stored Command: " + commands.back().command);
}
// Serial.println("Heap After Parsing: " + String(ESP.getFreeHeap()));
// Serial.println("--- Parsing Finished ---\n");
return commands;
}

7
src/classes/CommandTypes.h Normal file
View File

@ -0,0 +1,7 @@
enum CommandType{
Move,
Chain,
Power,
Speed,
Accel
};

6
src/classes/ICommand.h Normal file
View File

@ -0,0 +1,6 @@
#include "CommandTypes.h"
class ICommand {
public:
CommandType getCommandType();
};