Arduino Mega Control Shield

1/*
2  Arduino Mega Control Shield - All Motors Demo
3  Learn By Doing! - https://learnbydoing.dev
4
5  Spins all 10 stepper motors at the same time:
6  - 4 wheel motors driven by TB6600 drivers (common-anode wiring)
7  - 6 arm motors driven by a DRV8825 driver board
8  The motors reverse their direction every few seconds.
9  The 3 servos sweep back and forth at the same time.
10
11  Motor power (12-24 V) must be connected directly to the drivers.
12*/
13
14#include <Servo.h>
15
16// ================= Wheel drivers (TB6600, common anode) =================
17// With common-anode wiring the logic is inverted:
18// ENA- HIGH = no current in the optocoupler = driver ENABLED
19const int NUM_WHEELS = 4;
20const int WHEEL_EN[NUM_WHEELS]  = {30, 36, 42, 48};  // FL, BL, FR, BR
21const int WHEEL_STP[NUM_WHEELS] = {32, 38, 44, 50};
22const int WHEEL_DIR[NUM_WHEELS] = {34, 40, 46, 52};
23const int WHEEL_ENABLED = HIGH;
24
25// ================= Arm drivers (DRV8825) =================
26// DRV8825 ENABLE is active LOW
27const int NUM_ARM = 6;
28const int ARM_EN[NUM_ARM]  = {13, 8, 2, 5, 27, 25};  // M1..M6
29const int ARM_STP[NUM_ARM] = {12, 9, 3, 6, 26, 22};
30const int ARM_DIR[NUM_ARM] = {11, 10, 4, 7, 24, 23};
31const int ARM_ENABLED = LOW;
32
33// ================= Servos =================
34const int NUM_SERVOS = 3;
35const int SERVO_PIN[NUM_SERVOS] = {31, 33, 35};
36Servo servos[NUM_SERVOS];
37
38// ================= Motion settings =================
39// Time between two step pulses, in microseconds (lower = faster)
40const unsigned long WHEEL_STEP_INTERVAL_US = 600;
41const unsigned long ARM_STEP_INTERVAL_US   = 900;
42
43// Time before reversing direction, in milliseconds
44const unsigned long REVERSE_INTERVAL_MS = 2000;
45
46// Servo sweep settings
47const unsigned long SERVO_UPDATE_MS = 15;  // time between servo updates
48const int SERVO_MIN_ANGLE = 30;
49const int SERVO_MAX_ANGLE = 150;
50
51// ================= State variables =================
52unsigned long lastWheelStep = 0;
53unsigned long lastArmStep = 0;
54unsigned long lastReverse = 0;
55unsigned long lastServoUpdate = 0;
56
57bool wheelStepState = LOW;
58bool armStepState = LOW;
59bool direction = HIGH;
60
61int servoAngle = 90;
62int servoIncrement = 1;
63
64// Sets the direction pins of all the stepper drivers
65void setDirection(bool dir) {
66  for (int i = 0; i < NUM_WHEELS; i++) {
67    digitalWrite(WHEEL_DIR[i], dir);
68  }
69  for (int i = 0; i < NUM_ARM; i++) {
70    digitalWrite(ARM_DIR[i], dir);
71  }
72}
73
74void setup() {
75  Serial.begin(115200);
76  Serial.println(F("Arduino Mega Control Shield - All Motors Demo"));
77
78  // Wheel drivers
79  for (int i = 0; i < NUM_WHEELS; i++) {
80    pinMode(WHEEL_EN[i], OUTPUT);
81    pinMode(WHEEL_STP[i], OUTPUT);
82    pinMode(WHEEL_DIR[i], OUTPUT);
83    digitalWrite(WHEEL_STP[i], LOW);
84    digitalWrite(WHEEL_EN[i], WHEEL_ENABLED);
85  }
86
87  // Arm drivers
88  for (int i = 0; i < NUM_ARM; i++) {
89    pinMode(ARM_EN[i], OUTPUT);
90    pinMode(ARM_STP[i], OUTPUT);
91    pinMode(ARM_DIR[i], OUTPUT);
92    digitalWrite(ARM_STP[i], LOW);
93    digitalWrite(ARM_EN[i], ARM_ENABLED);
94  }
95
96  // Servos
97  for (int i = 0; i < NUM_SERVOS; i++) {
98    servos[i].attach(SERVO_PIN[i]);
99    servos[i].write(servoAngle);
100  }
101
102  setDirection(direction);
103
104  // Give the drivers time to wake up
105  delay(500);
106
107  unsigned long nowUs = micros();
108  lastWheelStep = nowUs;
109  lastArmStep = nowUs;
110  lastReverse = millis();
111  lastServoUpdate = millis();
112
113  Serial.println(F("Motors running..."));
114}
115
116void loop() {
117  unsigned long nowUs = micros();
118  unsigned long nowMs = millis();
119
120  // ---- Wheel motors: toggle the STEP pins (two toggles = one step) ----
121  if (nowUs - lastWheelStep >= WHEEL_STEP_INTERVAL_US / 2) {
122    lastWheelStep = nowUs;
123    wheelStepState = !wheelStepState;
124    for (int i = 0; i < NUM_WHEELS; i++) {
125      digitalWrite(WHEEL_STP[i], wheelStepState);
126    }
127  }
128
129  // ---- Arm motors: toggle the STEP pins ----
130  if (nowUs - lastArmStep >= ARM_STEP_INTERVAL_US / 2) {
131    lastArmStep = nowUs;
132    armStepState = !armStepState;
133    for (int i = 0; i < NUM_ARM; i++) {
134      digitalWrite(ARM_STP[i], armStepState);
135    }
136  }
137
138  // ---- Reverse direction periodically ----
139  if (nowMs - lastReverse >= REVERSE_INTERVAL_MS) {
140    lastReverse = nowMs;
141    direction = !direction;
142
143    // Short pause so the motors don't reverse at full speed
144    delay(200);
145    setDirection(direction);
146    delayMicroseconds(10);
147
148    Serial.print(F("Direction: "));
149    Serial.println(direction ? F("forward") : F("backward"));
150
151    // Reset the timers after the pause
152    lastWheelStep = micros();
153    lastArmStep = micros();
154  }
155
156  // ---- Servo sweep ----
157  if (nowMs - lastServoUpdate >= SERVO_UPDATE_MS) {
158    lastServoUpdate = nowMs;
159    servoAngle += servoIncrement;
160    if (servoAngle >= SERVO_MAX_ANGLE || servoAngle <= SERVO_MIN_ANGLE) {
161      servoIncrement = -servoIncrement;
162    }
163    for (int i = 0; i < NUM_SERVOS; i++) {
164      servos[i].write(servoAngle);
165    }
166  }
167}