Good Evening,
I've been trying to adapt my PID motor controller code to start only when signalled by a bluetooth character. Currently, the PID controller relies on each loop iteration and rotates a motor cw for 1m in 10 secs to a predetermined target at 0.1m/s and then ccw at the same speed to 0m and looped.
Instead of relying on the loop, I'm trying to signal the sequence to start but no matter what I try I cannot find a solution. If I start the sequence after the running for say 5 secs, my motor will only rotate cw for 5 secs then on to ccw for 10 before looping correctly.
Can anyone suggest any changes I can make to my code (below) which would allow the sequence to start when signalled by the bluetooth character.
#include <util/atomic.h>
#define ENCA 2 // YELLOW
#define ENCB 3 // WHITE
#define IN2 9
#define IN1 10
char getstr;
int pos = 0;
volatile int posi = 0;
long prevT = 0;
long currT = 0;
float eprev = 0;
float eintegral = 0;
float deltaT;
float t;
// targets
float target_f = 0;
long target = 0;
float velocity; // m/s
void setTarget(float t, float deltat) {
float positionChange = 0;
float pulsesPerTurn = 4 * 131;
float pulsesPerMeter = pulsesPerTurn * 18.85;
t = fmod(t, 20); // time is in seconds
if (t < 10) {
positionChange = velocity * deltat * pulsesPerMeter;
}
else if (t < 20) {
positionChange = -velocity * deltat * pulsesPerMeter;
}
target_f = target_f + positionChange;
target = (long) target_f;
}
void setup() {
Serial.begin(9600);
pinMode(ENCA, INPUT_PULLUP);
pinMode(ENCB, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(ENCA), readEncoder, RISING);
pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
}
void loop() {
checkForMessage();
// time difference
currT = micros();
deltaT = ((float) (currT - prevT)) / ( 1.0e6 );
prevT = currT;
// PID constants
float kp = 0.5;
float kd = 0.25;
float ki = 0.0;
setTarget(currT / 1.0e6, deltaT);
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
pos = posi;
}
// error
int e = pos - target;
// derivative
float dedt = (e - eprev) / (deltaT);
// integral
eintegral = eintegral + e * deltaT;
// control signal
float u = kp * e + kd * dedt + ki * eintegral;
// motor power
float pwr = fabs(u);
if ( pwr > 255 ) {
pwr = 255;
}
// motor direction
int dir = 1;
if (u < 0) {
dir = -1;
}
// signal the motor
setMotor(dir, pwr, IN1, IN2);
// store previous error
eprev = e;
}
void setMotor(int dir, int pwmVal, int in1, int in2) {
if (dir == 1) {
digitalWrite(in2, LOW);
analogWrite(in1, pwmVal);
}
else if (dir == -1) {
digitalWrite(in1, LOW);
analogWrite(in2, pwmVal);
}
else {
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
}
}
void readEncoder() {
int b = digitalRead(ENCB);
if (b > 0) {
posi++;
}
else {
posi--;
}
}
void checkForMessage() {
if (Serial.available()) {
getstr = Serial.read();
}
if (getstr == 'a') {
velocity = 0.1; // m/s
}
}