Test 6-Axis Robot (3D Printed)

#include <AccelStepper.h>
#include <Servo.h>

// ===== PIN DEFINITIONS =====

// Joint 1 (J1) - TMC2208
#define J1_STEP A6
#define J1_DIR  A7
#define J1_EN   A2

// Joint 2 (J2) - Dual DM542T motors
// Motor A
#define J2A_PUL_MINUS 23
#define J2A_PUL_PLUS  16
#define J2A_DIR_MINUS 17
#define J2A_DIR_PLUS  25
// Motor B
#define J2B_PUL_MINUS 31
#define J2B_PUL_PLUS  33
#define J2B_DIR_MINUS 29
#define J2B_DIR_PLUS  27

// Joint 3 (J3) - DM542T
#define J3_PUL_MINUS 39
#define J3_PUL_PLUS  41
#define J3_DIR_MINUS 37
#define J3_DIR_PLUS  35

// Joint 4 (J4) - TMC2208
#define J4_STEP 36
#define J4_DIR  34
#define J4_EN   30

// Joint 5 (J5) - TMC2208
#define J5_STEP A0
#define J5_DIR  A1
#define J5_EN   38

// Joint 6 (J6) - TMC2208
#define J6_STEP 26
#define J6_DIR  28
#define J6_EN   24

// Gripper Servo
#define GRIPPER_PIN 4  // Connected to D4

// ===== USER CONFIGURATION =====

// Steps per revolution for each joint
const float STEPS_PER_REV[6] = {
  16850,  // J1
  4200,   // J2
  4200,   // J3
  1600,   // J4
  7200,   // J5
  1600    // J6
};

// Maximum speed for each joint (in revolutions per second)
const float MAX_SPEED_REV_PER_SEC[6] = {
  0.1,    // J1
  0.05,   // J2
  0.1,    // J3
  0.25,   // J4
  0.25,   // J5
  0.25    // J6
};

// Acceleration for each joint (in revolutions per second^2)
const float ACCELERATION_REV_PER_SEC2[6] = {
  0.2,    // J1
  0.1,    // J2
  0.2,    // J3
  0.5,    // J4
  0.5,    // J5
  0.5     // J6
};

// Gripper servo limits (adjusted for your physical device)
const int MIN_GRIPPER_ANGLE = 70;
const int MAX_GRIPPER_ANGLE = 160;

// ===== STEPPER OBJECTS =====

// TMC2208 joints (J1, J4, J6 reversed direction)
AccelStepper j1(AccelStepper::DRIVER, J1_STEP, J1_DIR);
AccelStepper j4(AccelStepper::DRIVER, J4_STEP, J4_DIR);
AccelStepper j5(AccelStepper::DRIVER, J5_STEP, J5_DIR);
AccelStepper j6(AccelStepper::DRIVER, J6_STEP, J6_DIR);

// DM542T joints (J2, J3 reversed direction)
AccelStepper j2a(AccelStepper::DRIVER, J2A_PUL_PLUS, J2A_DIR_PLUS);
AccelStepper j2b(AccelStepper::DRIVER, J2B_PUL_PLUS, J2B_DIR_PLUS);
AccelStepper j3(AccelStepper::DRIVER, J3_PUL_PLUS, J3_DIR_PLUS);

// Servo object
Servo gripper;

// Array of stepper pointers for easy iteration
AccelStepper* steppers[] = {&j1, &j2a, &j3, &j4, &j5, &j6};

// Current target angles in degrees (absolute positions)
float targetAngles[6] = {0, 0, 0, 0, 0, 0};

// Current gripper position
int currentGripperAngle = 160;  // Start at maximum position

String buffer = "";

// ===== SETUP =====
void setup() {
  Serial.begin(115200);
  
  // ===== TMC2208 Configuration (J1, J4, J5, J6) =====
  pinMode(J1_EN, OUTPUT);
  pinMode(J4_EN, OUTPUT);
  pinMode(J5_EN, OUTPUT);
  pinMode(J6_EN, OUTPUT);
  
  // Enable TMC2208 drivers (LOW = enabled)
  digitalWrite(J1_EN, LOW);
  digitalWrite(J4_EN, LOW);
  digitalWrite(J5_EN, LOW);
  digitalWrite(J6_EN, LOW);
  
  // ===== DM542T Configuration =====
  // J2 Motor A
  pinMode(J2A_PUL_MINUS, OUTPUT);
  pinMode(J2A_DIR_MINUS, OUTPUT);
  digitalWrite(J2A_PUL_MINUS, LOW);
  digitalWrite(J2A_DIR_MINUS, LOW);
  
  // J2 Motor B
  pinMode(J2B_PUL_MINUS, OUTPUT);
  pinMode(J2B_DIR_MINUS, OUTPUT);
  digitalWrite(J2B_PUL_MINUS, LOW);
  digitalWrite(J2B_DIR_MINUS, LOW);
  
  // J3
  pinMode(J3_PUL_MINUS, OUTPUT);
  pinMode(J3_DIR_MINUS, OUTPUT);
  digitalWrite(J3_PUL_MINUS, LOW);
  digitalWrite(J3_DIR_MINUS, LOW);
  
  // ===== Gripper Servo Configuration =====
  gripper.attach(GRIPPER_PIN);
  gripper.write(currentGripperAngle);
  delay(500);  // Give servo time to reach starting position
  
  // Configure stepper parameters for all joints
  for (int i = 0; i < 6; i++) {
    // Convert rev/sec to steps/sec
    float maxSpeedSteps = MAX_SPEED_REV_PER_SEC[i] * STEPS_PER_REV[i];
    float accelSteps = ACCELERATION_REV_PER_SEC2[i] * STEPS_PER_REV[i];
    
    steppers[i]->setMaxSpeed(maxSpeedSteps);
    steppers[i]->setAcceleration(accelSteps);
    steppers[i]->setCurrentPosition(0);
  }
  
  // Reverse direction for J1, J2, J3, J4, J6 by inverting direction pin
  j1.setPinsInverted(true, false, false);   // J1 reversed
  j2a.setPinsInverted(true, false, false);  // J2A reversed
  j3.setPinsInverted(true, false, false);   // J3 reversed
  j4.setPinsInverted(true, false, false);   // J4 reversed
  j6.setPinsInverted(true, false, false);   // J6 reversed
  
  // Configure J2B (motor B) with same settings as J2A
  float j2MaxSpeedSteps = MAX_SPEED_REV_PER_SEC[1] * STEPS_PER_REV[1];
  float j2AccelSteps = ACCELERATION_REV_PER_SEC2[1] * STEPS_PER_REV[1];
  j2b.setMaxSpeed(j2MaxSpeedSteps);
  j2b.setAcceleration(j2AccelSteps);
  j2b.setCurrentPosition(0);
  j2b.setPinsInverted(true, false, false);  // J2B reversed
  
  // Print startup information
  Serial.println("========================================");
  Serial.println("6-AXIS ROBOT MANUAL CONTROL - ANGLE MODE");
  Serial.println("WITH GRIPPER SERVO CONTROL");
  Serial.println("========================================");
  Serial.println();
  Serial.println("Commands:");
  Serial.println("  J1 <angle>  - Move Joint 1 to angle (degrees)");
  Serial.println("  J2 <angle>  - Move Joint 2 to angle (degrees)");
  Serial.println("  J3 <angle>  - Move Joint 3 to angle (degrees)");
  Serial.println("  J4 <angle>  - Move Joint 4 to angle (degrees)");
  Serial.println("  J5 <angle>  - Move Joint 5 to angle (degrees)");
  Serial.println("  J6 <angle>  - Move Joint 6 to angle (degrees)");
  Serial.println("  G <angle>   - Move Gripper to angle (70-160 degrees)");
  Serial.println("  HOME        - Return all joints to 0 degrees");
  Serial.println("  STATUS      - Show current joint positions");
  Serial.println();
  Serial.println("Examples:");
  Serial.println("  J1 90       - Move Joint 1 to +90 degrees");
  Serial.println("  J1 -90      - Move Joint 1 to -90 degrees");
  Serial.println("  J2 45       - Move Joint 2 to 45 degrees");
  Serial.println("  G 100       - Move Gripper to 100 degrees");
  Serial.println();
  Serial.println("Configuration:");
  Serial.println("  Joint | Steps/Rev | Max Speed | Acceleration");
  Serial.println("  ------|-----------|-----------|-------------");
  for (int i = 0; i < 6; i++) {
    Serial.print("  J");
    Serial.print(i + 1);
    Serial.print("   | ");
    Serial.print((int)STEPS_PER_REV[i]);
    Serial.print("     | ");
    Serial.print(MAX_SPEED_REV_PER_SEC[i], 2);
    Serial.print(" rev/s | ");
    Serial.print(ACCELERATION_REV_PER_SEC2[i], 2);
    Serial.println(" rev/s^2");
  }
  Serial.println();
  Serial.println("Gripper Configuration:");
  Serial.print("  Range: ");
  Serial.print(MIN_GRIPPER_ANGLE);
  Serial.print("-");
  Serial.print(MAX_GRIPPER_ANGLE);
  Serial.println(" degrees");
  Serial.print("  Starting Position: ");
  Serial.print(currentGripperAngle);
  Serial.println(" degrees");
  Serial.println();
  Serial.println("Special Notes:");
  Serial.println("  - J2 has dual motors (opposite directions)");
  Serial.println("  - All angles are ABSOLUTE positions");
  Serial.println("  - Positions relative to startup (0,0,0,0,0,0)");
  Serial.println("  - Gripper uses servo on pin D4");
  Serial.println();
  Serial.println("========================================");
  Serial.println("Ready for commands...");
  Serial.println();
}

// ===== MAIN LOOP =====
void loop() {
  // Receive commands
  if (Serial.available()) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') {
      if (buffer.length() > 0) {
        processCommand(buffer);
        buffer = "";
      }
    } else {
      buffer += c;
    }
  }
  
  // Run all steppers (non-blocking)
  for (int i = 0; i < 6; i++) {
    steppers[i]->run();
  }
  
  // Run J2B (motor B with opposite direction)
  j2b.run();
}

// ===== PROCESS INCOMING COMMANDS =====
void processCommand(String cmd) {
  cmd.trim();
  cmd.toUpperCase();
  
  // Check for STATUS command
  if (cmd == "STATUS") {
    printStatus();
    return;
  }
  
  // Check for HOME command
  if (cmd == "HOME") {
    homeAllJoints();
    return;
  }
  
  // Check for Gripper command (G <angle>)
  if (cmd.startsWith("G")) {
    // Extract angle value
    int spaceIndex = cmd.indexOf(' ');
    if (spaceIndex == -1) {
      Serial.println("ERROR: No angle specified. Format: G <angle>");
      return;
    }
    
    String angleStr = cmd.substring(spaceIndex + 1);
    angleStr.trim();
    int angle = angleStr.toInt();
    
    moveGripper(angle);
    return;
  }
  
  // Parse joint commands (format: J1 90 or J1 -90)
  if (cmd.startsWith("J")) {
    // Extract joint number
    int jointNum = -1;
    if (cmd.length() >= 2) {
      char jointChar = cmd.charAt(1);
      if (jointChar >= '1' && jointChar <= '6') {
        jointNum = jointChar - '1';  // Convert to 0-based index
      }
    }
    
    if (jointNum == -1) {
      Serial.println("ERROR: Invalid joint number. Use J1-J6.");
      return;
    }
    
    // Extract angle value
    int spaceIndex = cmd.indexOf(' ');
    if (spaceIndex == -1) {
      Serial.println("ERROR: No angle specified. Format: J1 <angle>");
      return;
    }
    
    String angleStr = cmd.substring(spaceIndex + 1);
    angleStr.trim();
    float angle = angleStr.toFloat();
    
    // Move the joint
    moveJoint(jointNum, angle);
  } else {
    Serial.println("ERROR: Unknown command. Use J1-J6 <angle>, G <angle>, HOME, or STATUS.");
  }
}

// ===== MOVE GRIPPER =====
void moveGripper(int angle) {
  // Validate angle is within limits
  if (angle >= MIN_GRIPPER_ANGLE && angle <= MAX_GRIPPER_ANGLE) {
    currentGripperAngle = angle;
    gripper.write(currentGripperAngle);
    
    Serial.println("----------------------------------");
    Serial.println("Gripper");
    Serial.print("  Moved to: ");
    Serial.print(currentGripperAngle);
    Serial.println(" degrees");
    Serial.println("----------------------------------");
  } else {
    Serial.println("----------------------------------");
    Serial.print("ERROR: Gripper angle out of range. Use ");
    Serial.print(MIN_GRIPPER_ANGLE);
    Serial.print("-");
    Serial.print(MAX_GRIPPER_ANGLE);
    Serial.println(" degrees");
    Serial.println("----------------------------------");
  }
}

// ===== MOVE SPECIFIC JOINT =====
void moveJoint(int jointIndex, float angleDegrees) {
  if (jointIndex < 0 || jointIndex >= 6) {
    Serial.println("ERROR: Invalid joint index.");
    return;
  }
  
  // Update target angle
  targetAngles[jointIndex] = angleDegrees;
  
  // Calculate target steps from angle
  // angle (degrees) -> steps = (angle / 360) * steps_per_rev
  float revolutions = angleDegrees / 360.0;
  long targetSteps = (long)(revolutions * STEPS_PER_REV[jointIndex]);
  
  // Move the stepper
  steppers[jointIndex]->moveTo(targetSteps);
  
  // For J2, also move the second motor in opposite direction
  if (jointIndex == 1) {  // J2 (index 1)
    j2b.moveTo(-targetSteps);  // Opposite direction
  }
  
  // Print detailed confirmation
  Serial.println("----------------------------------");
  Serial.print("Joint J");
  Serial.println(jointIndex + 1);
  Serial.print("  Target Angle: ");
  Serial.print(angleDegrees, 2);
  Serial.println(" degrees");
  Serial.print("  Target Steps: ");
  Serial.println(targetSteps);
  Serial.print("  Current Position: ");
  Serial.print(steppers[jointIndex]->currentPosition());
  Serial.println(" steps");
  Serial.print("  Distance to Move: ");
  Serial.print(steppers[jointIndex]->distanceToGo());
  Serial.println(" steps");
  
  if (jointIndex == 1) {
    Serial.println("  Note: J2 dual motors moving in opposite directions");
  }
  
  Serial.println("----------------------------------");
}

// ===== HOME ALL JOINTS =====
void homeAllJoints() {
  Serial.println("Homing all joints to 0 degrees...");
  
  for (int i = 0; i < 6; i++) {
    targetAngles[i] = 0;
    steppers[i]->moveTo(0);
  }
  
  // Home J2B as well
  j2b.moveTo(0);
  
  Serial.println("All joints moving to home position.");
  Serial.println("Note: Gripper position unchanged. Use 'G <angle>' to adjust gripper.");
}

// ===== PRINT STATUS =====
void printStatus() {
  Serial.println();
  Serial.println("========== CURRENT STATUS ==========");
  
  for (int i = 0; i < 6; i++) {
    long currentSteps = steppers[i]->currentPosition();
    long targetSteps = steppers[i]->targetPosition();
    float currentAngle = (currentSteps / STEPS_PER_REV[i]) * 360.0;
    
    Serial.print("J");
    Serial.print(i + 1);
    Serial.print(": Target=");
    Serial.print(targetAngles[i], 2);
    Serial.print("°, Current=");
    Serial.print(currentAngle, 2);
    Serial.print("°, Steps=");
    Serial.print(currentSteps);
    Serial.print("/");
    Serial.print(targetSteps);
    
    if (steppers[i]->distanceToGo() == 0) {
      Serial.println(" [AT TARGET]");
    } else {
      Serial.print(" [MOVING, ");
      Serial.print(steppers[i]->distanceToGo());
      Serial.println(" steps to go]");
    }
    
    // Show J2B status as well
    if (i == 1) {
      Serial.print("   J2B: Position=");
      Serial.print(j2b.currentPosition());
      Serial.print(" steps");
      if (j2b.distanceToGo() == 0) {
        Serial.println(" [AT TARGET]");
      } else {
        Serial.print(" [MOVING, ");
        Serial.print(j2b.distanceToGo());
        Serial.println(" steps to go]");
      }
    }
  }
  
  // Show gripper status
  Serial.print("Gripper: ");
  Serial.print(currentGripperAngle);
  Serial.println("° [SERVO]");
  
  Serial.println("====================================");
  Serial.println();
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

#include <AccelStepper.h>

#include <Servo.h>

// ===== PIN DEFINITIONS =====

// Joint 1 (J1) - TMC2208

#define J1_STEP A6

#define J1_DIR A7

#define J1_EN A2

// Joint 2 (J2) - Dual DM542T motors

// Motor A

#define J2A_PUL_MINUS 23

#define J2A_PUL_PLUS 16

#define J2A_DIR_MINUS 17

#define J2A_DIR_PLUS 25

// Motor B

#define J2B_PUL_MINUS 31

#define J2B_PUL_PLUS 33

#define J2B_DIR_MINUS 29

#define J2B_DIR_PLUS 27

// Joint 3 (J3) - DM542T

#define J3_PUL_MINUS 39

#define J3_PUL_PLUS 41

#define J3_DIR_MINUS 37

#define J3_DIR_PLUS 35

// Joint 4 (J4) - TMC2208

#define J4_STEP 36

#define J4_DIR 34

#define J4_EN 30

// Joint 5 (J5) - TMC2208

#define J5_STEP A0

#define J5_DIR A1

#define J5_EN 38

// Joint 6 (J6) - TMC2208

#define J6_STEP 26

#define J6_DIR 28

#define J6_EN 24

// Gripper Servo

#define GRIPPER_PIN 4 // Connected to D4

// ===== USER CONFIGURATION =====

// Steps per revolution for each joint

const float STEPS_PER_REV[6] = {

16850, // J1

4200, // J2

4200, // J3

1600, // J4

7200, // J5

1600 // J6

};

// Maximum speed for each joint (in revolutions per second)

const float MAX_SPEED_REV_PER_SEC[6] = {

0.1, // J1

0.05, // J2

0.1, // J3

0.25, // J4

0.25, // J5

0.25 // J6

};

// Acceleration for each joint (in revolutions per second^2)

const float ACCELERATION_REV_PER_SEC2[6] = {

0.2, // J1

0.1, // J2

0.2, // J3

0.5, // J4

0.5, // J5

0.5 // J6

};

// Gripper servo limits (adjusted for your physical device)

const int MIN_GRIPPER_ANGLE = 70;

const int MAX_GRIPPER_ANGLE = 160;

// ===== STEPPER OBJECTS =====

// TMC2208 joints (J1, J4, J6 reversed direction)

AccelStepper j1(AccelStepper::DRIVER, J1_STEP, J1_DIR);

AccelStepper j4(AccelStepper::DRIVER, J4_STEP, J4_DIR);

AccelStepper j5(AccelStepper::DRIVER, J5_STEP, J5_DIR);

AccelStepper j6(AccelStepper::DRIVER, J6_STEP, J6_DIR);

// DM542T joints (J2, J3 reversed direction)

AccelStepper j2a(AccelStepper::DRIVER, J2A_PUL_PLUS, J2A_DIR_PLUS);

AccelStepper j2b(AccelStepper::DRIVER, J2B_PUL_PLUS, J2B_DIR_PLUS);

AccelStepper j3(AccelStepper::DRIVER, J3_PUL_PLUS, J3_DIR_PLUS);

// Servo object

Servo gripper;

// Array of stepper pointers for easy iteration

AccelStepper* steppers[] = {&j1, &j2a, &j3, &j4, &j5, &j6};

// Current target angles in degrees (absolute positions)

float targetAngles[6] = {0, 0, 0, 0, 0, 0};

// Current gripper position

int currentGripperAngle = 160; // Start at maximum position

String buffer = "";

// ===== SETUP =====

void setup() {

Serial.begin(115200);

// ===== TMC2208 Configuration (J1, J4, J5, J6) =====

pinMode(J1_EN, OUTPUT);

pinMode(J4_EN, OUTPUT);

pinMode(J5_EN, OUTPUT);

pinMode(J6_EN, OUTPUT);

// Enable TMC2208 drivers (LOW = enabled)

digitalWrite(J1_EN, LOW);

digitalWrite(J4_EN, LOW);

digitalWrite(J5_EN, LOW);

digitalWrite(J6_EN, LOW);

// ===== DM542T Configuration =====

// J2 Motor A

pinMode(J2A_PUL_MINUS, OUTPUT);

pinMode(J2A_DIR_MINUS, OUTPUT);

digitalWrite(J2A_PUL_MINUS, LOW);

digitalWrite(J2A_DIR_MINUS, LOW);

// J2 Motor B

pinMode(J2B_PUL_MINUS, OUTPUT);

pinMode(J2B_DIR_MINUS, OUTPUT);

digitalWrite(J2B_PUL_MINUS, LOW);

digitalWrite(J2B_DIR_MINUS, LOW);

// J3

pinMode(J3_PUL_MINUS, OUTPUT);

pinMode(J3_DIR_MINUS, OUTPUT);

digitalWrite(J3_PUL_MINUS, LOW);

digitalWrite(J3_DIR_MINUS, LOW);

// ===== Gripper Servo Configuration =====

gripper.attach(GRIPPER_PIN);

gripper.write(currentGripperAngle);

delay(500); // Give servo time to reach starting position

// Configure stepper parameters for all joints

for (int i = 0; i < 6; i++) {

// Convert rev/sec to steps/sec

float maxSpeedSteps = MAX_SPEED_REV_PER_SEC[i] * STEPS_PER_REV[i];

float accelSteps = ACCELERATION_REV_PER_SEC2[i] * STEPS_PER_REV[i];

steppers[i]->setMaxSpeed(maxSpeedSteps);

steppers[i]->setAcceleration(accelSteps);

steppers[i]->setCurrentPosition(0);

}

// Reverse direction for J1, J2, J3, J4, J6 by inverting direction pin

j1.setPinsInverted(true, false, false); // J1 reversed

j2a.setPinsInverted(true, false, false); // J2A reversed

j3.setPinsInverted(true, false, false); // J3 reversed

j4.setPinsInverted(true, false, false); // J4 reversed

j6.setPinsInverted(true, false, false); // J6 reversed

// Configure J2B (motor B) with same settings as J2A

float j2MaxSpeedSteps = MAX_SPEED_REV_PER_SEC[1] * STEPS_PER_REV[1];

float j2AccelSteps = ACCELERATION_REV_PER_SEC2[1] * STEPS_PER_REV[1];

j2b.setMaxSpeed(j2MaxSpeedSteps);

j2b.setAcceleration(j2AccelSteps);

j2b.setCurrentPosition(0);

j2b.setPinsInverted(true, false, false); // J2B reversed

// Print startup information

Serial.println("========================================");

Serial.println("6-AXIS ROBOT MANUAL CONTROL - ANGLE MODE");

Serial.println("WITH GRIPPER SERVO CONTROL");

Serial.println("========================================");

Serial.println();

Serial.println("Commands:");

Serial.println(" J1 <angle> - Move Joint 1 to angle (degrees)");

Serial.println(" J2 <angle> - Move Joint 2 to angle (degrees)");

Serial.println(" J3 <angle> - Move Joint 3 to angle (degrees)");

Serial.println(" J4 <angle> - Move Joint 4 to angle (degrees)");

Serial.println(" J5 <angle> - Move Joint 5 to angle (degrees)");

Serial.println(" J6 <angle> - Move Joint 6 to angle (degrees)");

Serial.println(" G <angle> - Move Gripper to angle (70-160 degrees)");

Serial.println(" HOME - Return all joints to 0 degrees");

Serial.println(" STATUS - Show current joint positions");

Serial.println();

Serial.println("Examples:");

Serial.println(" J1 90 - Move Joint 1 to +90 degrees");

Serial.println(" J1 -90 - Move Joint 1 to -90 degrees");

Serial.println(" J2 45 - Move Joint 2 to 45 degrees");

Serial.println(" G 100 - Move Gripper to 100 degrees");

Serial.println();

Serial.println("Configuration:");

Serial.println(" Joint | Steps/Rev | Max Speed | Acceleration");

Serial.println(" ------|-----------|-----------|-------------");

for (int i = 0; i < 6; i++) {

Serial.print(" J");

Serial.print(i + 1);

Serial.print(" | ");

Serial.print((int)STEPS_PER_REV[i]);

Serial.print(" | ");

Serial.print(MAX_SPEED_REV_PER_SEC[i], 2);

Serial.print(" rev/s | ");

Serial.print(ACCELERATION_REV_PER_SEC2[i], 2);

Serial.println(" rev/s^2");

}

Serial.println();

Serial.println("Gripper Configuration:");

Serial.print(" Range: ");

Serial.print(MIN_GRIPPER_ANGLE);

Serial.print("-");

Serial.print(MAX_GRIPPER_ANGLE);

Serial.println(" degrees");

Serial.print(" Starting Position: ");

Serial.print(currentGripperAngle);

Serial.println(" degrees");

Serial.println();

Serial.println("Special Notes:");

Serial.println(" - J2 has dual motors (opposite directions)");

Serial.println(" - All angles are ABSOLUTE positions");

Serial.println(" - Positions relative to startup (0,0,0,0,0,0)");

Serial.println(" - Gripper uses servo on pin D4");

Serial.println();

Serial.println("========================================");

Serial.println("Ready for commands...");

Serial.println();

}

// ===== MAIN LOOP =====

void loop() {

// Receive commands

if (Serial.available()) {

char c = Serial.read();

if (c == '\n' || c == '\r') {

if (buffer.length() > 0) {

processCommand(buffer);

buffer = "";

}

} else {

buffer += c;

}

// Run all steppers (non-blocking)

for (int i = 0; i < 6; i++) {

steppers[i]->run();

}

// Run J2B (motor B with opposite direction)

j2b.run();

}

// ===== PROCESS INCOMING COMMANDS =====

void processCommand(String cmd) {

cmd.trim();

cmd.toUpperCase();

// Check for STATUS command

if (cmd == "STATUS") {

printStatus();

return;

}

// Check for HOME command

if (cmd == "HOME") {

homeAllJoints();

return;

}

// Check for Gripper command (G <angle>)

if (cmd.startsWith("G")) {

// Extract angle value

int spaceIndex = cmd.indexOf(' ');

if (spaceIndex == -1) {

Serial.println("ERROR: No angle specified. Format: G <angle>");

return;

}

String angleStr = cmd.substring(spaceIndex + 1);

angleStr.trim();

int angle = angleStr.toInt();

moveGripper(angle);

return;

}

// Parse joint commands (format: J1 90 or J1 -90)

if (cmd.startsWith("J")) {

// Extract joint number

int jointNum = -1;

if (cmd.length() >= 2) {

char jointChar = cmd.charAt(1);

if (jointChar >= '1' && jointChar <= '6') {

jointNum = jointChar - '1'; // Convert to 0-based index

}

if (jointNum == -1) {

Serial.println("ERROR: Invalid joint number. Use J1-J6.");

return;

}

// Extract angle value

int spaceIndex = cmd.indexOf(' ');

if (spaceIndex == -1) {

Serial.println("ERROR: No angle specified. Format: J1 <angle>");

return;

}

String angleStr = cmd.substring(spaceIndex + 1);

angleStr.trim();

float angle = angleStr.toFloat();

// Move the joint

moveJoint(jointNum, angle);

} else {

Serial.println("ERROR: Unknown command. Use J1-J6 <angle>, G <angle>, HOME, or STATUS.");

}

// ===== MOVE GRIPPER =====

void moveGripper(int angle) {

// Validate angle is within limits

if (angle >= MIN_GRIPPER_ANGLE && angle <= MAX_GRIPPER_ANGLE) {

currentGripperAngle = angle;

gripper.write(currentGripperAngle);

Serial.println("----------------------------------");

Serial.println("Gripper");

Serial.print(" Moved to: ");

Serial.print(currentGripperAngle);

Serial.println(" degrees");

Serial.println("----------------------------------");

} else {

Serial.println("----------------------------------");

Serial.print("ERROR: Gripper angle out of range. Use ");

Serial.print(MIN_GRIPPER_ANGLE);

Serial.print("-");

Serial.print(MAX_GRIPPER_ANGLE);

Serial.println(" degrees");

Serial.println("----------------------------------");

}

// ===== MOVE SPECIFIC JOINT =====

void moveJoint(int jointIndex, float angleDegrees) {

if (jointIndex < 0 || jointIndex >= 6) {

Serial.println("ERROR: Invalid joint index.");

return;

}

// Update target angle

targetAngles[jointIndex] = angleDegrees;

// Calculate target steps from angle

// angle (degrees) -> steps = (angle / 360) * steps_per_rev

float revolutions = angleDegrees / 360.0;

long targetSteps = (long)(revolutions * STEPS_PER_REV[jointIndex]);

// Move the stepper

steppers[jointIndex]->moveTo(targetSteps);

// For J2, also move the second motor in opposite direction

if (jointIndex == 1) { // J2 (index 1)

j2b.moveTo(-targetSteps); // Opposite direction

}

// Print detailed confirmation

Serial.println("----------------------------------");

Serial.print("Joint J");

Serial.println(jointIndex + 1);

Serial.print(" Target Angle: ");

Serial.print(angleDegrees, 2);

Serial.println(" degrees");

Serial.print(" Target Steps: ");

Serial.println(targetSteps);

Serial.print(" Current Position: ");

Serial.print(steppers[jointIndex]->currentPosition());

Serial.println(" steps");

Serial.print(" Distance to Move: ");

Serial.print(steppers[jointIndex]->distanceToGo());

Serial.println(" steps");

if (jointIndex == 1) {

Serial.println(" Note: J2 dual motors moving in opposite directions");

}

Serial.println("----------------------------------");

}

// ===== HOME ALL JOINTS =====

void homeAllJoints() {

Serial.println("Homing all joints to 0 degrees...");

for (int i = 0; i < 6; i++) {

targetAngles[i] = 0;

steppers[i]->moveTo(0);

}

// Home J2B as well

j2b.moveTo(0);

Serial.println("All joints moving to home position.");

Serial.println("Note: Gripper position unchanged. Use 'G <angle>' to adjust gripper.");