Arduino Code for DIY Pick N Place

#include <SPI.h>

// Pin Definitions
const int CSN_PIN = 10;  // Chip select for TMC5272
const int Z_IN1 = 6;     // L298N input 1
const int Z_IN2 = 7;     // L298N input 2
const int Z_IN3 = 8;     // L298N input 3
const int Z_IN4 = 9;     // L298N input 4

// TMC5272 Register Addresses
#define GCONF       0x00
#define GSTAT       0x01
#define IHOLD_IRUN  0x10
#define CHOPCONF    0x6C
#define COOLCONF    0x6D
#define DCCTRL      0x6E
#define DRVSTATUS   0x6F
#define RAMPMODE    0x20
#define XACTUAL     0x21
#define VACTUAL     0x22
#define XTARGET     0x2D

// Motor configuration
const long stepsPerRevolution = 200;  // For NEMA 17
const float mmPerRevolution = 40.0;   // GT2 pulley circumference for X/Y
const float mmPerStep = 0.02;         // Lead screw pitch divided by steps for Z

// Current settings
const uint8_t runCurrent = 16;    // Range 0-31
const uint8_t holdCurrent = 8;    // Range 0-31

void setup() {
  Serial.begin(115200);

  // Initialize SPI
  SPI.begin();
  pinMode(CSN_PIN, OUTPUT);
  digitalWrite(CSN_PIN, HIGH);

  // Initialize L298N pins
  pinMode(Z_IN1, OUTPUT);
  pinMode(Z_IN2, OUTPUT);
  pinMode(Z_IN3, OUTPUT);
  pinMode(Z_IN4, OUTPUT);

  // Configure TMC5272
  initTMC5272();
}

void initTMC5272() {
  // Basic configuration
  writeTMC5272(GCONF, 0x00000004);      // Enable internal reference voltage
  writeTMC5272(CHOPCONF, 0x000100C3);    // Standard chopper config
  writeTMC5272(IHOLD_IRUN, 0x00071703);  // Set current values

  // Set motor parameters for X axis
  writeTMC5272(RAMPMODE, 0);             // Position mode
  writeTMC5272(XACTUAL, 0);              // Reset position

  delay(100);  // Allow time for driver to initialize
}

void writeTMC5272(uint8_t addr, uint32_t data) {
  digitalWrite(CSN_PIN, LOW);
  SPI.transfer(addr | 0x80);  // Set write bit
  SPI.transfer((data >> 24) & 0xFF);
  SPI.transfer((data >> 16) & 0xFF);
  SPI.transfer((data >> 8) & 0xFF);
  SPI.transfer(data & 0xFF);
  digitalWrite(CSN_PIN, HIGH);
}

uint32_t readTMC5272(uint8_t addr) {
  digitalWrite(CSN_PIN, LOW);
  SPI.transfer(addr);
  uint32_t data = 0;
  data |= SPI.transfer(0) << 24;
  data |= SPI.transfer(0) << 16;
  data |= SPI.transfer(0) << 8;
  data |= SPI.transfer(0);
  digitalWrite(CSN_PIN, HIGH);
  return data;
}

// Function to move X axis (connected to TMC5272)
void moveX(float mm) {
  long steps = mm / mmPerStep;
  uint32_t currentPos = readTMC5272(XACTUAL);
  uint32_t newPos = currentPos + steps;
  writeTMC5272(XTARGET, newPos);

  // Wait for movement to complete
  while(readTMC5272(XACTUAL) != newPos) {
    delay(1);
  }
}

// Function to move Y axis (connected to TMC5272)
void moveY(float mm) {
  // Similar to moveX, but using different registers for Y axis
  // Implementation depends on how Y axis is configured in TMC5272
}

// Z axis control using L298N
void moveZ(int direction, int steps) {
  const int stepDelay = 2;  // Adjust for desired speed

  for(int i = 0; i < steps; i++) {
    switch(direction) {
      case 1:  // Up
        digitalWrite(Z_IN1, HIGH);
        digitalWrite(Z_IN2, LOW);
        digitalWrite(Z_IN3, HIGH);
        digitalWrite(Z_IN4, LOW);
        delay(stepDelay);
        digitalWrite(Z_IN1, HIGH);
        digitalWrite(Z_IN2, LOW);
        digitalWrite(Z_IN3, LOW);
        digitalWrite(Z_IN4, HIGH);
        delay(stepDelay);
        digitalWrite(Z_IN1, LOW);
        digitalWrite(Z_IN2, HIGH);
        digitalWrite(Z_IN3, LOW);
        digitalWrite(Z_IN4, HIGH);
        delay(stepDelay);
        digitalWrite(Z_IN1, LOW);
        digitalWrite(Z_IN2, HIGH);
        digitalWrite(Z_IN3, HIGH);
        digitalWrite(Z_IN4, LOW);
        delay(stepDelay);
        break;

      case 0:  // Down
        digitalWrite(Z_IN1, LOW);
        digitalWrite(Z_IN2, HIGH);
        digitalWrite(Z_IN3, HIGH);
        digitalWrite(Z_IN4, LOW);
        delay(stepDelay);
        digitalWrite(Z_IN1, LOW);
        digitalWrite(Z_IN2, HIGH);
        digitalWrite(Z_IN3, LOW);
        digitalWrite(Z_IN4, HIGH);
        delay(stepDelay);
        digitalWrite(Z_IN1, HIGH);
        digitalWrite(Z_IN2, LOW);
        digitalWrite(Z_IN3, LOW);
        digitalWrite(Z_IN4, HIGH);
        delay(stepDelay);
        digitalWrite(Z_IN1, HIGH);
        digitalWrite(Z_IN2, LOW);
        digitalWrite(Z_IN3, HIGH);
        digitalWrite(Z_IN4, LOW);
        delay(stepDelay);
        break;
    }
  }
  // Turn off all coils
  digitalWrite(Z_IN1, LOW);
  digitalWrite(Z_IN2, LOW);
  digitalWrite(Z_IN3, LOW);
  digitalWrite(Z_IN4, LOW);
}

void loop() {
  // Example movement sequence
  if(Serial.available()) {
    char cmd = Serial.read();
    switch(cmd) {
      case 'x':
        moveX(10);  // Move X axis 10mm
        break;
      case 'y':
        moveY(10);  // Move Y axis 10mm
        break;
      case 'u':
        moveZ(1, 200);  // Move Z up 200 steps
        break;
      case 'd':
        moveZ(0, 200);  // Move Z down 200 steps
        break;
    }
  }
}