Repository navigation
Expand file tree
/
Copy pathmain.cpp
More file actions
209 lines (178 loc) · 7.58 KB
/
Copy pathmain.cpp
File metadata and controls
209 lines (178 loc) · 7.58 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
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
// Tobble - integrated control loop: sense -> think -> act, both motors.
//
// sense - fused tilt angle from the MPU6500 (complementary filter)
// think - PID turns tilt error into a signed motor command
// act - TB6612FNG drives BOTH motors with that command
//
// ORIENTATION: the IMU is mounted vertically on the standing bot. Measured raw
// accel showed forward/back tilt lives on the X-Z plane (ax swings, az ~ -g) and
// the Y gyro axis - NOT the Y-Z / X the flat code assumed. So the angle is
// atan2(ax, -az) and the gyro rate is gy. A small offset (BALANCE_OFFSET) is
// subtracted so upright reads ~0 instead of ~180 (which sat on the atan2
// wraparound and made the angle jump). It balances at the gains below,
// tethered to USB for power; untethered on the battery pack is untested.
//
// IMU: MPU6500 (WHO_AM_I = 0x70), read directly over I2C. Gyro already deg/s
// (raw/131) - no rad/s conversion. atan2 returns radians, so * 180/PI.
#include <Arduino.h>
#include <Wire.h>
// ---------------- IMU (I2C) ----------------
const uint8_t MPU = 0x68;
const uint8_t PWR_MGMT_1 = 0x6B;
const uint8_t ACCEL_XOUT_H = 0x3B;
// ---------------- Motor driver (TB6612) ----------------
// Channel A (one wheel)
const int PWMA = 25; // speed (PWM)
const int AIN1 = 26; // direction
const int AIN2 = 27; // direction
// Channel B (other wheel)
const int PWMB = 14; // speed (PWM)
const int BIN1 = 32; // direction
const int BIN2 = 13; // direction
// The two motors sit mirrored on the chassis, so the same command can spin them
// opposite ways. If the bot spins in place instead of driving straight, flip
// this to true (or swap one motor's output wires).
const bool MOTOR_B_INVERT = true; // motors are mirrored on the chassis
// ledc PWM - one channel per motor
const int PWM_CH_A = 0;
const int PWM_CH_B = 1;
const int PWM_FREQ = 1000; // Hz
const int PWM_RES = 8; // bits -> duty 0..255
const int MAX_CMD = 220; // command clamp - raised from 180 (2026-08-28): 180
// couldn't arrest a ~20 deg lean in time. Wheels are
// free-spinning (not stalled) at this level, so it's
// safe; only lower it again if a motor starts running hot.
// ---------------- Complementary filter ----------------
const float ALPHA = 0.98; // 98% gyro, 2% accel
const float GYRO_SIGN = 1.0; // flip to -1.0 if fused angle diverges from accel
const float BALANCE_OFFSET = 1.5; // deg subtracted so upright reads ~0. Recalibrated
// 2026-08-28 (was 7.0) after rewiring shifted the
// IMU slightly. Nudge to find the
// true balance point during tuning.
float angle = 0.0; // fused tilt angle (deg), ~0 at upright
float gyroBias = 0.0; // resting gyro offset (deg/s)
unsigned long lastTime = 0;
// ---------------- PID ----------------
// Gains - being tuned on hardware. Ki stays 0 until P and D are dialed in.
// 2026-08-28: raised Kp from 25 - small leans barely got a response (25 * 3deg
// = only 75/220 command), so it kept falling until the lean was already large,
// then overcorrected hard. Kp=60 reacts harder to small leans, catching them
// sooner instead of needing a big lean to produce a big command.
float Kp = 60.0, Ki = 0.0, Kd = 0.8;
const float TARGET_ANGLE = 0.0; // upright (angle is offset so this is 0)
const float MAX_I = 150.0; // integral clamp (anti-windup, for when Ki>0)
const float FALL_LIMIT = 45.0; // deg past which the bot is "fallen" -> stop
float errorSum = 0.0;
float lastError = 0.0;
// ---------------- IMU helpers ----------------
void writeReg(uint8_t reg, uint8_t val) {
Wire.beginTransmission(MPU);
Wire.write(reg);
Wire.write(val);
Wire.endTransmission();
}
// Reads accel-x/z (m/s^2) and gyro-y (deg/s) - the axes that carry the bot's
// forward/back tilt in this mounting.
void readSensor(float &ax, float &az, float &gy) {
Wire.beginTransmission(MPU);
Wire.write(ACCEL_XOUT_H);
Wire.endTransmission(false);
Wire.requestFrom((int)MPU, 14);
uint8_t buf[14];
for (int i = 0; i < 14; i++) buf[i] = Wire.read();
int16_t rawAx = (buf[0] << 8) | buf[1]; // accel X
int16_t rawAz = (buf[4] << 8) | buf[5]; // accel Z
int16_t rawGy = (buf[10] << 8) | buf[11]; // gyro Y
ax = rawAx / 16384.0 * 9.81; // +-2g -> 16384 LSB/g
az = rawAz / 16384.0 * 9.81;
gy = rawGy / 131.0; // +-250 dps -> 131 LSB/(deg/s), already deg/s
}
// tilt angle from accel: forward/back lives on the X-Z plane, upright offset
// removed so it reads ~0.
float accelTilt(float ax, float az) {
return atan2(ax, -az) * 180.0 / PI - BALANCE_OFFSET;
}
// Average many still samples to learn the resting gyro offset.
float calibrateGyroBias(int samples) {
float sum = 0.0;
for (int i = 0; i < samples; i++) {
float ax, az, gy;
readSensor(ax, az, gy);
sum += gy;
delay(3);
}
return sum / samples;
}
// ---------------- Motor ----------------
// Drive one channel: sign = direction, magnitude = speed.
void driveChannel(int in1, int in2, int ch, int command) {
if (command >= 0) {
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
} else {
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
command = -command;
}
ledcWrite(ch, command);
}
// Drive both motors from one command (channel B mirrored if MOTOR_B_INVERT).
void driveMotors(int command) {
driveChannel(AIN1, AIN2, PWM_CH_A, command);
driveChannel(BIN1, BIN2, PWM_CH_B, MOTOR_B_INVERT ? -command : command);
}
void setup() {
Serial.begin(115200);
delay(1000);
Wire.begin(21, 22);
Wire.setClock(100000);
delay(100);
writeReg(PWR_MGMT_1, 0x00); // wake the MPU6500
delay(100);
pinMode(AIN1, OUTPUT); pinMode(AIN2, OUTPUT);
pinMode(BIN1, OUTPUT); pinMode(BIN2, OUTPUT);
ledcSetup(PWM_CH_A, PWM_FREQ, PWM_RES); ledcAttachPin(PWMA, PWM_CH_A);
ledcSetup(PWM_CH_B, PWM_FREQ, PWM_RES); ledcAttachPin(PWMB, PWM_CH_B);
driveMotors(0);
Serial.println("Calibrating gyro - keep still...");
gyroBias = calibrateGyroBias(500);
float ax, az, gy; // seed angle from the accelerometer
readSensor(ax, az, gy);
angle = accelTilt(ax, az);
lastTime = millis();
Serial.println("Control loop running.");
}
void loop() {
float ax, az, gy;
readSensor(ax, az, gy);
// --- sense: fused tilt angle ---
float accelAngle = accelTilt(ax, az); // ~0 upright, +/- for back/forward
float gyroRate = GYRO_SIGN * (gy - gyroBias); // already deg/s
unsigned long now = millis();
float dt = (now - lastTime) / 1000.0;
if (dt <= 0) dt = 0.001;
lastTime = now;
angle = ALPHA * (angle + gyroRate * dt) + (1.0 - ALPHA) * accelAngle;
// --- think: PID ---
float error = TARGET_ANGLE - angle;
errorSum += error * dt;
errorSum = constrain(errorSum, -MAX_I, MAX_I); // anti-windup
float dError = (error - lastError) / dt;
lastError = error;
float output = Kp * error + Ki * errorSum + Kd * dError;
output = constrain(output, -MAX_CMD, MAX_CMD);
// --- act: drive both motors (stop if the bot has toppled) ---
int command = (int)output;
if (fabs(angle) > FALL_LIMIT) command = 0;
driveMotors(command);
// --- verify over serial (throttled so it's readable while tilting) ---
static unsigned long lastPrint = 0;
if (now - lastPrint >= 100) {
lastPrint = now;
Serial.print("angle: "); Serial.print(angle, 1);
Serial.print(" cmd: "); Serial.print(command);
Serial.print(" ");
Serial.println(command == 0 ? "STOP" : (command > 0 ? "FWD" : "REV"));
}
delay(10);
}