133 lines
2.4 KiB
C++
133 lines
2.4 KiB
C++
#include <Arduino.h>
|
|
|
|
#define ENCA 2
|
|
#define ENCB 3
|
|
|
|
#define MOTORA 7
|
|
#define MOTORB 8
|
|
#define MOTOREN 9
|
|
|
|
uint8_t state = 0;
|
|
uint8_t lastState = 0;
|
|
|
|
int32_t theta = 0;
|
|
int32_t targetTheta = 100;
|
|
|
|
int32_t error = 0;
|
|
int32_t lastError = 0;
|
|
int32_t dError = 0;
|
|
int32_t iError = 0;
|
|
int32_t lastInstant = 0;
|
|
int32_t instant;
|
|
float Kp = 10;
|
|
float Kd = 0;
|
|
float Ki = 0;
|
|
|
|
bool f = false;
|
|
|
|
int32_t period = 10000;
|
|
|
|
void update() {
|
|
delay(1);
|
|
lastInstant = instant;
|
|
instant = micros();
|
|
int32_t deltaT = instant - lastInstant;
|
|
lastError = error;
|
|
error = targetTheta - theta;
|
|
dError = (error - lastError)/deltaT;
|
|
iError = iError + error*deltaT;
|
|
if (iError > 255)
|
|
iError = 255;
|
|
else if (iError < -255)
|
|
iError = -255;
|
|
}
|
|
|
|
int16_t pid() { return Kp * error + Ki * iError + Kd * dError; }
|
|
|
|
void setPower(int16_t power) {
|
|
if (power > 255) {
|
|
power = 255;
|
|
} else if (power < -255) {
|
|
power = -255;
|
|
}
|
|
|
|
if (power > 0) {
|
|
analogWrite(MOTOREN, power);
|
|
digitalWrite(MOTORA, HIGH);
|
|
digitalWrite(MOTORB, LOW);
|
|
} else {
|
|
analogWrite(MOTOREN, -power);
|
|
digitalWrite(MOTORA, LOW);
|
|
digitalWrite(MOTORB, HIGH);
|
|
}
|
|
}
|
|
|
|
void plot(){
|
|
Serial.print(">timestamp:");
|
|
Serial.println(micros());
|
|
Serial.print(">theta:");
|
|
Serial.println(theta);
|
|
Serial.print(">targetTheta:");
|
|
Serial.println(targetTheta);
|
|
}
|
|
|
|
void encoderIsr() {
|
|
lastState = state;
|
|
state = (digitalRead(3) << 1) + digitalRead(2);
|
|
switch (lastState) {
|
|
case 0:
|
|
if (state == 1) {
|
|
theta++;
|
|
} else {
|
|
theta--;
|
|
}
|
|
break;
|
|
case 1:
|
|
if (state == 3) {
|
|
theta++;
|
|
} else {
|
|
theta--;
|
|
}
|
|
break;
|
|
case 3:
|
|
if (state == 2) {
|
|
theta++;
|
|
} else {
|
|
theta--;
|
|
}
|
|
break;
|
|
case 2:
|
|
if (state == 0) {
|
|
theta++;
|
|
} else {
|
|
theta--;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
pinMode(LED_BUILTIN, OUTPUT);
|
|
pinMode(ENCA, INPUT);
|
|
pinMode(ENCB, INPUT);
|
|
pinMode(MOTORA, OUTPUT);
|
|
pinMode(MOTORB, OUTPUT);
|
|
pinMode(MOTOREN, OUTPUT);
|
|
attachInterrupt(digitalPinToInterrupt(ENCA), encoderIsr, CHANGE);
|
|
attachInterrupt(digitalPinToInterrupt(ENCB), encoderIsr, CHANGE);
|
|
}
|
|
|
|
void loop() {
|
|
if (!f && millis() % period < period/2) {
|
|
targetTheta = 100;
|
|
f = true;
|
|
} else if (f && millis() % period > period/2) {
|
|
targetTheta = -100;
|
|
f = false;
|
|
}
|
|
plot();
|
|
update();
|
|
setPower(pid());
|
|
delay(10);
|
|
} |