113 lines
2.7 KiB
C++
113 lines
2.7 KiB
C++
#include "TimerOne.h"
|
|
#include "U8glib.h"
|
|
|
|
// Drive the motor
|
|
const byte IN1 = 10;
|
|
const byte IN2 = 11;
|
|
const byte ENABLE = 6;
|
|
const byte ENCO_B = 3;
|
|
const byte ENCO_A = 2;
|
|
|
|
double output_level = 0;
|
|
|
|
// PID
|
|
double err = 0, diff_err = 0, accumulate_err = 0;
|
|
const double Kp = 2, Ki = 0, Kd = 6;
|
|
const unsigned long DO_PID_CONTROL_IN_US = 1000;
|
|
|
|
// Settings for the level
|
|
long goal_step = 0;
|
|
long current_step = 0;
|
|
|
|
const int DISTANCE_BETWEEN_LEVELS = 50;
|
|
const int NUMBER_OF_LEVELS = 5;
|
|
const long MAX_GOAL_STEP = DISTANCE_BETWEEN_LEVELS * NUMBER_OF_LEVELS;
|
|
|
|
// OLED Args and initialization
|
|
U8GLIB_SSD1306_128X32 u8g(U8G_I2C_OPT_NONE);
|
|
|
|
const byte OLED_WIDTH = 128;
|
|
const byte OLED_HEIGHT = 32;
|
|
const byte PROGESS_BAR_HEIGHT = 8;
|
|
|
|
void onEncoAChange() {
|
|
if (digitalRead(ENCO_A) == digitalRead(ENCO_B)) {
|
|
current_step++;
|
|
} else {
|
|
current_step--;
|
|
}
|
|
|
|
if ((current_step - goal_step) % DISTANCE_BETWEEN_LEVELS >
|
|
DISTANCE_BETWEEN_LEVELS / 2 &&
|
|
goal_step < MAX_GOAL_STEP) {
|
|
goal_step += DISTANCE_BETWEEN_LEVELS;
|
|
accumulate_err = 0;
|
|
}
|
|
if ((goal_step - current_step) % DISTANCE_BETWEEN_LEVELS >
|
|
DISTANCE_BETWEEN_LEVELS / 2 &&
|
|
goal_step > 0) {
|
|
goal_step -= DISTANCE_BETWEEN_LEVELS;
|
|
accumulate_err = 0;
|
|
}
|
|
}
|
|
|
|
void doPIDControl() {
|
|
diff_err = goal_step - current_step - err;
|
|
err = goal_step - current_step;
|
|
accumulate_err += err;
|
|
|
|
output_level = Kp * err + Ki * accumulate_err + Kd * diff_err;
|
|
|
|
if (output_level > 255) {
|
|
output_level = 255;
|
|
} else if (output_level < -255) {
|
|
output_level = -255;
|
|
}
|
|
|
|
if (output_level < 0) {
|
|
digitalWrite(IN1, HIGH);
|
|
digitalWrite(IN2, LOW);
|
|
analogWrite(ENABLE, -output_level);
|
|
} else {
|
|
digitalWrite(IN1, LOW);
|
|
digitalWrite(IN2, HIGH);
|
|
analogWrite(ENABLE, output_level);
|
|
}
|
|
}
|
|
|
|
void drawOLED() {
|
|
u8g.firstPage();
|
|
do {
|
|
u8g.drawFrame(0, (OLED_HEIGHT - PROGESS_BAR_HEIGHT) / 2, OLED_WIDTH,
|
|
PROGESS_BAR_HEIGHT);
|
|
u8g.drawBox(2, (OLED_HEIGHT - PROGESS_BAR_HEIGHT) / 2 + 2,
|
|
(OLED_WIDTH - 4) * goal_step / MAX_GOAL_STEP,
|
|
PROGESS_BAR_HEIGHT - 4);
|
|
} while (u8g.nextPage());
|
|
}
|
|
|
|
void setup() {
|
|
pinMode(IN1, OUTPUT);
|
|
pinMode(IN2, OUTPUT);
|
|
pinMode(ENABLE, OUTPUT);
|
|
digitalWrite(IN1, HIGH);
|
|
digitalWrite(IN2, LOW);
|
|
pinMode(ENCO_B, INPUT);
|
|
pinMode(ENCO_A, INPUT);
|
|
Timer1.initialize();
|
|
Timer1.setPeriod(DO_PID_CONTROL_IN_US);
|
|
Timer1.attachInterrupt(doPIDControl);
|
|
attachInterrupt(digitalPinToInterrupt(ENCO_A), onEncoAChange, CHANGE);
|
|
Serial.begin(115200);
|
|
}
|
|
|
|
void loop() {
|
|
// Serial.print("Goal: ");
|
|
Serial.print('$');
|
|
Serial.print(goal_step);
|
|
// Serial.print(" Current: ");
|
|
Serial.print(", ");
|
|
Serial.print(current_step);
|
|
Serial.print(";");
|
|
drawOLED();
|
|
} |