297 أسطر
8.0 KiB
C++
297 أسطر
8.0 KiB
C++
// AStarPIDTest - Test the PID control for an AStar-based robot.
|
|
//
|
|
// To gather PID tuning data for graphing, put the robot on blocks, then
|
|
// start this program and open a Serial Monitor. Press S4 to start both
|
|
// motors, let them come to a steady speed, then press S4 to stop the
|
|
// motors. Copy the Serial output to a spreadsheet and plot target speed and
|
|
// current speed versus time. You should see a classic PID controller graph
|
|
// with some overshoot.
|
|
|
|
#include <AStar32U4.h>
|
|
#include <EnableInterrupt.h>
|
|
#include <SimplePID.h>
|
|
|
|
// Motor parameters. These are based on a Pololu #2285 motor with 48cpr
|
|
// encoder.
|
|
const float GEAR_RATIO = 210.59;
|
|
const float ENCODER_CPR = 12;
|
|
|
|
// Number of encoder ticks per revolution of the wheel.
|
|
const float TICKS_PER_REV = GEAR_RATIO * ENCODER_CPR;
|
|
|
|
// Maximum desired motor speed. Pololu #2285 are rated for 130rpm, but we
|
|
// don't drive them with enough voltage to achieve that.
|
|
const float MAX_REVS_PER_SEC = 100.0 / 60.0;
|
|
|
|
// Use 60% of maximum speed for PID tuning.
|
|
const int PID_TUNING_TICKS_PER_SEC = 0.6 * MAX_REVS_PER_SEC * TICKS_PER_REV;
|
|
|
|
// DFRobot Romeo v2 and BLE PIN assignments.
|
|
|
|
const int ONBOARD_SWITCH_PIN = A7;
|
|
|
|
// Pins for the Pololu motor encoder outputs.
|
|
const int M1_A = 7;
|
|
const int M1_B = 11;
|
|
const int M2_A = 15;
|
|
const int M2_B = 16;
|
|
|
|
// Minimum motor control value. Motor output below this will stall.
|
|
const int MIN_MOTOR_CMD = 10;
|
|
|
|
// Maximum motor control value.
|
|
const int MAX_MOTOR_CMD = 400;
|
|
|
|
AStar32U4Motors motors;
|
|
|
|
// These objects provide access to the A-Star's on-board
|
|
// buttons. We will only use buttonA.
|
|
AStar32U4ButtonA buttonA;
|
|
AStar32U4ButtonB buttonB;
|
|
AStar32U4ButtonC buttonC;
|
|
|
|
enum { NO_BUTTON, BUTTON_A, BUTTON_B, BUTTON_C };
|
|
|
|
int leftSpeedTarget = 0;
|
|
int rightSpeedTarget = 0;
|
|
|
|
volatile long leftTicks;
|
|
volatile long rightTicks;
|
|
|
|
long lastLeftTicks = 0;
|
|
long lastRightTicks = 0;
|
|
|
|
int leftMotorCmd = 0;
|
|
int rightMotorCmd = 0;
|
|
|
|
unsigned long lastLoopTime;
|
|
|
|
unsigned long lastSwitchTime;
|
|
|
|
float loopTime = 0.0;
|
|
|
|
// Ziegler-Nichols tuning. See this Wikipedia article for details:
|
|
// https://en.wikipedia.org/wiki/PID_controller#Loop_tuning
|
|
// To tune the controller, set USE_KU_ONLY to 1 and increase Ku
|
|
// until oscillation occurs, and set Tu to the oscillation period
|
|
// in seconds. Then set USE_KU_ONLY to zero to use the
|
|
// Ziegler-Nichols tune. To get a less agressive tune, set LESS_AGGRESSIVE
|
|
// to 1.
|
|
|
|
// Set to 1 to use Ku only, to determine oscillation point.
|
|
#define USE_KU_ONLY 0
|
|
|
|
const float Ku = .15;
|
|
const float Tu = .1142857143;
|
|
|
|
#define MANUAL_TUNE 1
|
|
#define LESS_AGGRESSIVE 0
|
|
|
|
#if MANUAL_TUNE
|
|
// Found empirically to rapidly converge with little overshoot.
|
|
// There is no integral constant, but not needed when driving
|
|
// on flat ground, and when the heading is otherwise controlled
|
|
// by higher-level software. (I.e., there's no need to catch up
|
|
// for past errors.)
|
|
const float Kp = 0.07;
|
|
const float Ki = 0.0;
|
|
const float Kd = 0.001;
|
|
#elif !LESS_AGGRESSIVE
|
|
const float Kp = 0.6*Ku;
|
|
const float Ki = 2*Kp/Tu;
|
|
const float Kd = Kp*Tu/8;
|
|
#else
|
|
const float Kp = 0.2*Ku;
|
|
const float Ki = 2*Kp/Tu;
|
|
const float Kd = Kp*Tu/3;
|
|
#endif
|
|
|
|
#if USE_KU_ONLY
|
|
SimplePID leftController = SimplePID(Ku, 0, 0);
|
|
SimplePID rightController = SimplePID(Ku, 0, 0);
|
|
#else
|
|
SimplePID leftController = SimplePID(Kp, Ki, Kd);
|
|
SimplePID rightController = SimplePID(Kp, Ki, Kd);
|
|
#endif
|
|
|
|
// Sets up the serial output and the motor control pins, and attaches
|
|
// interrupt handlers to the pins on which we will read the encoder
|
|
// phototransistor values.
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
delay(3000);
|
|
|
|
enableInterrupt(M1_A, leftAChange, CHANGE);
|
|
enableInterrupt(M1_B, leftBChange, CHANGE);
|
|
enableInterrupt(M2_A, rightAChange, CHANGE);
|
|
enableInterrupt(M2_B, rightBChange, CHANGE);
|
|
|
|
// Serial.print("Time (s)\t");
|
|
Serial.print("Left Target\tLeft Speed");
|
|
// Serial.print("\tLeft Cum Error\tLeft Motor\t");
|
|
Serial.print("\tRight Target\tRight Speed");
|
|
// Serial.print("\tRight Cum Error\tRight Motor");
|
|
Serial.println();
|
|
|
|
lastLoopTime = micros();
|
|
lastSwitchTime = millis();
|
|
|
|
leftTicks = rightTicks = 0;
|
|
}
|
|
|
|
// Loops forever showing the motor speeds and errors since the last loop,
|
|
// and adjusts the target speeds depending on whether the user is pressing
|
|
// switches S2 through S5, as indicated in the code below.
|
|
void loop() {
|
|
delay(15);
|
|
|
|
unsigned long curLoopTime = micros();
|
|
noInterrupts();
|
|
long curLeftTicks = leftTicks;
|
|
long curRightTicks = rightTicks;
|
|
interrupts();
|
|
|
|
float dt = (curLoopTime - lastLoopTime) / 1E6;
|
|
|
|
float leftSpeed = (curLeftTicks - lastLeftTicks) / dt;
|
|
float rightSpeed = (curRightTicks - lastRightTicks) / dt;
|
|
|
|
int leftControl = leftController.getControlValue(leftSpeed, dt);
|
|
leftMotorCmd += min(MAX_MOTOR_CMD, leftControl);
|
|
leftMotorCmd = constrain(leftMotorCmd, -MAX_MOTOR_CMD, MAX_MOTOR_CMD);
|
|
if (leftMotorCmd > 0) {
|
|
leftMotorCmd = max(leftMotorCmd, MIN_MOTOR_CMD);
|
|
}
|
|
|
|
int rightControl = rightController.getControlValue(rightSpeed, dt);
|
|
rightMotorCmd += min(MAX_MOTOR_CMD, rightControl);
|
|
rightMotorCmd = constrain(rightMotorCmd, -MAX_MOTOR_CMD, MAX_MOTOR_CMD);
|
|
if (rightMotorCmd > 0) {
|
|
rightMotorCmd = max(rightMotorCmd, MIN_MOTOR_CMD);
|
|
}
|
|
|
|
// Coast to a stop if target is zero.
|
|
if (leftSpeedTarget == 0) {
|
|
leftMotorCmd = 0;
|
|
}
|
|
if (rightSpeedTarget == 0) {
|
|
rightMotorCmd = 0;
|
|
}
|
|
|
|
setSpeed(leftMotorCmd, rightMotorCmd);
|
|
|
|
if (leftSpeedTarget > 0 || leftSpeed > 0 || rightSpeedTarget > 0 || rightSpeed > 0) {
|
|
// Serial.print(loopTime, 3);
|
|
// Serial.print("\t");
|
|
Serial.print(leftSpeedTarget);
|
|
Serial.print("\t");
|
|
Serial.print(leftSpeed);
|
|
Serial.print("\t");
|
|
// Serial.print(leftController.getCumulativeError());
|
|
// Serial.print("\t");
|
|
// Serial.print(leftMotorCmd);
|
|
// Serial.print("\t");
|
|
|
|
Serial.print(rightSpeedTarget);
|
|
Serial.print("\t");
|
|
Serial.print(rightSpeed);
|
|
Serial.print("\t");
|
|
// Serial.print(rightController.getCumulativeError());
|
|
// Serial.print("\t");
|
|
// Serial.print(rightMotorCmd);
|
|
// Serial.print("\t");
|
|
|
|
Serial.println();
|
|
|
|
loopTime += dt;
|
|
}
|
|
|
|
// Ignore switches for a short time after pressing, to avoid bounce.
|
|
if (curLoopTime - lastSwitchTime > 0.5*1E6) {
|
|
int switchValue = readSwitch();
|
|
if (switchValue > 0) {
|
|
lastSwitchTime = curLoopTime;
|
|
|
|
switch (switchValue) {
|
|
case BUTTON_A:
|
|
// Left motor on/off.
|
|
leftSpeedTarget = (leftSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
|
|
break;
|
|
case BUTTON_B:
|
|
// Right motor on/off.
|
|
rightSpeedTarget = (rightSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
|
|
break;
|
|
case BUTTON_C:
|
|
// Both motors on/off.
|
|
leftSpeedTarget = (leftSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
|
|
rightSpeedTarget = (rightSpeedTarget==0 ? PID_TUNING_TICKS_PER_SEC : 0);
|
|
break;
|
|
default:
|
|
// no change in speed
|
|
break;
|
|
}
|
|
|
|
leftController.setSetPoint(leftSpeedTarget);
|
|
rightController.setSetPoint(rightSpeedTarget);
|
|
}
|
|
}
|
|
|
|
lastLeftTicks = curLeftTicks;
|
|
lastRightTicks = curRightTicks;
|
|
lastLoopTime = curLoopTime;
|
|
}
|
|
|
|
void waitForSwitch1() {
|
|
while (readSwitch() != 1) {
|
|
// do nothing
|
|
}
|
|
}
|
|
|
|
void setSpeed(int leftSpeed, int rightSpeed) {
|
|
motors.setSpeeds(leftSpeed, rightSpeed);
|
|
}
|
|
|
|
void leftAChange() {
|
|
if (digitalRead(M1_A) == digitalRead(M1_B)) {
|
|
++leftTicks;
|
|
} else {
|
|
--leftTicks;
|
|
}
|
|
}
|
|
|
|
void leftBChange() {
|
|
if (digitalRead(M1_A) != digitalRead(M1_B)) {
|
|
++leftTicks;
|
|
} else {
|
|
--leftTicks;
|
|
}
|
|
}
|
|
|
|
void rightAChange() {
|
|
if (digitalRead(M2_A) != digitalRead(M2_B)) {
|
|
++rightTicks;
|
|
} else {
|
|
--rightTicks;
|
|
}
|
|
}
|
|
|
|
void rightBChange() {
|
|
if (digitalRead(M2_A) == digitalRead(M2_B)) {
|
|
++rightTicks;
|
|
} else {
|
|
--rightTicks;
|
|
}
|
|
}
|
|
|
|
int readSwitch() {
|
|
if (buttonA.getSingleDebouncedRelease()) {
|
|
return BUTTON_A;
|
|
} else if (buttonB.getSingleDebouncedRelease()) {
|
|
return BUTTON_B;
|
|
} else if (buttonC.getSingleDebouncedRelease()) {
|
|
return BUTTON_C;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|