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

Compare commits

base: BOTAlex:testing-rtos-controlling-steppers
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. "testing-rtos-controlling-steppers" and "master" have entirely different histories.
testing-rt ... master

12 changed files with 220 additions and 428 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

91
src/ClawMachineOverhaul.ino
View File

@ -1,33 +1,84 @@
#include <Arduino.h> #include "StepperController.h"
#ifndef ESP32 #include "StepperXController.h"
#include "Time.h"
#include <Arduino_FreeRTOS.h> #include <Arduino_FreeRTOS.h>
#endif
#include "MotorControlPart.h"
#include "ZCodeParser.h"
#include "Shared/MotorMoveParams.h"
void anotherFunc(void * params){ #define JoystickPin 63
MotorMoveParams *pam = (MotorMoveParams *)malloc(sizeof(MotorMoveParams));
pam->motorChar = 'x';
pam->moveTimeMs = 3000;
xTaskCreate(MoveMotor, "Motor", 1024, (void *)pam, 10, NULL);
vTaskDelay(pdMS_TO_TICKS(50)); // Allow time for other task to accept params #define grabPin 19
free(pam);
vTaskDelete(NULL); Time time;
}
void setup(){ void setup(){
// Start serial communication at 115200 baud
Serial.begin(115200); Serial.begin(115200);
setupMotor(); // Set the pin mode for the defined pins
pinMode(grabPin, INPUT_PULLUP);
// Create tasks initSteppers();
xTaskCreate(anotherFunc, "Other loop", 1024, NULL, 5, NULL); 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(){ void loop(){
// The RTOS scheduler manages the tasks. No code is needed here. // Updates deltatime
time.update();
// ZCodeParser::ParseString("G0 E20 X40; Comment test\nG1 Y50 X69\nG0 Y235 E5532"); 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();
} }

209
src/MotorControlPart.h
View File

@ -1,209 +0,0 @@
#include <Arduino.h>
#ifndef ESP32
#include <Arduino_FreeRTOS.h>
#endif
#include <Shared/ZCommand.h>
#include <Shared/MotorMoveParams.h>
#include <math.h>
// Pin used for the joystick
#define JOYSTICK_PIN 63
struct CharMapping {
char character;
int value;
};
// [X, Y, Head] TODO:
#define NUM_MOTORS 3
static const uint8_t STEP_PINS[NUM_MOTORS] = {54, 60, 36};
static const uint8_t DIR_PINS[NUM_MOTORS] = {55, 61, 34};
static const uint8_t EN_PINS[NUM_MOTORS] = {38, 56, 30};
bool motorsEnabled[NUM_MOTORS] = {false, false, false};
CharMapping mappings[] = {{'x', 0}, {'y', 1}, {'e', 2}};
uint8_t selectedIndex = 1;
// Critical range for detecting joystick movement
static const int JOY_CRITICAL_POINT = 25;
bool leftMovement = false;
bool rightMovement = false;
#ifdef ESP32
// Timer Handles
hw_timer_t *timer1 = NULL;
hw_timer_t *timer2 = NULL;
hw_timer_t *timer3 = NULL;
// Interrupt Service Routines for each timer
void IRAM_ATTR timer1ISR()
{
if (motorsEnabled[0])
{
digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0]));
}
}
void IRAM_ATTR timer2ISR()
{
if (motorsEnabled[1])
{
digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1]));
}
}
void IRAM_ATTR timer3ISR()
{
if (motorsEnabled[2])
{
digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2]));
}
}
#else
// Timers for stepping the motors
ISR(TIMER3_COMPA_vect) {
if (motorsEnabled[0]) {
digitalWrite(STEP_PINS[0], !digitalRead(STEP_PINS[0]));
}
}
ISR(TIMER4_COMPA_vect) {
if (motorsEnabled[1]) {
digitalWrite(STEP_PINS[1], !digitalRead(STEP_PINS[1]));
}
}
ISR(TIMER5_COMPA_vect) {
if (motorsEnabled[2]) {
digitalWrite(STEP_PINS[2], !digitalRead(STEP_PINS[2]));
}
}
#endif
int charToMotorIndex(char c) {
for (int i = 0; i < NUM_MOTORS; i++) {
if (mappings[i].character == c) {
return mappings[i].value;
}
}
return -1; // Not found
}
void updateInput() {
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);
motorsEnabled[selectedIndex] = leftMovement || rightMovement;
digitalWrite(EN_PINS[selectedIndex], !motorsEnabled[selectedIndex]); // Inverted for some reason
digitalWrite(DIR_PINS[selectedIndex], leftMovement);
// // Debug
// Serial.print("Raw: "); Serial.print(joystickValueRaw);
// Serial.print(" | Mapped: "); Serial.print(joystickValue);
// Serial.print(" | Left: "); Serial.print(leftMovement);
// Serial.print(" | Right: "); Serial.print(rightMovement);
// Serial.print(" | Motor: "); Serial.println(motorsEnabled[selectedIndex] ? "ON" : "OFF");
}
void ExecuteCommand(void *params){
ZCommand receivedValue = *((ZCommand *)params);
}
void MoveMotor(void *params) {
MotorMoveParams *motorParams = ((MotorMoveParams *) params);
char motorChar = motorParams->motorChar;
float moveTimeMs = motorParams->moveTimeMs;
int index = charToMotorIndex(motorChar);
int isBackwards = signbit(moveTimeMs); // Negative means move backwards, positive means move forward.
// Enable motor and set direction
motorsEnabled[index] = true;
digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason
digitalWrite(DIR_PINS[index], isBackwards);
vTaskDelay(pdMS_TO_TICKS(moveTimeMs));
// Stop motor
motorsEnabled[index] = false;
digitalWrite(EN_PINS[index], !motorsEnabled[index]); // Inverted for some reason
vTaskDelete(NULL);
}
void MotorControlTask(void *params) {
while (true) {
updateInput();
// Check if there is serial data available
while (Serial.available() > 0) {
String input = Serial.readStringUntil('\n'); // Read input until newline
input.trim(); // Remove any whitespace
// Check if the input matches the expected format ":0", ":1", or ":2"
if (input.length() == 2 && input[0] == ':') {
char indexChar = input[1];
if (indexChar >= '0' && indexChar <= '2') {
selectedIndex = indexChar - '0'; // Convert character to integer
Serial.print("Selected index set to: ");
Serial.println(selectedIndex);
} else {
Serial.println("Invalid index! Use :0, :1, or :2");
}
}
}
vTaskDelay(pdMS_TO_TICKS(50)); // Poll every 50ms
}
}
// Configures Timer1 for compare match interrupts
void setupMotor() {
for (size_t i = 0; i < NUM_MOTORS; i++)
{
pinMode(STEP_PINS[i], OUTPUT);
pinMode(DIR_PINS[i], OUTPUT);
pinMode(EN_PINS[i], 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 << WGM42) | (1 << CS50); // CTC mode, no prescaler
OCR5A = 499; // Compare match value
TIMSK5 |= (1 << OCIE5A); // Enable Timer1 Compare A Match Interrupt
sei();
#endif
}

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

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

76
src/StepperController.h Normal file
View File

@ -0,0 +1,76 @@
#include <SpeedyStepper.h>
#define NUM_STEPPERS 3
// [X, Y, Head]
const int stepPins[NUM_STEPPERS] = {60, 36};
const int dirPins[NUM_STEPPERS] = {61, 34};
const int enablePins[NUM_STEPPERS] = {56, 30};
enum StepperAxis{
Y,
Head
};
SpeedyStepper steppers[NUM_STEPPERS];
bool isMoving[NUM_STEPPERS];
// 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();
};