#define HIGH_SPEED_MODE 0 // set if mill is in high speed gear setting
#if HIGH_SPEED_MODE
#define MREV (18 * 4 * (30 / 14.0) * (21 / 20.0)) // 162
#else
#define MREV (18 * 4 * (30 / 14.0) * (29 / 12.0)) // 372.857143
#endif
#define GREV (12 * 4 * 56.0)
#define NTEETH 23
#define M 2.5 // motor power limit, 2.5 = full power
#define MOTORPWM 13
#define MOTORA 12
#define MOTORB 11
#define GPOS1 10
#define GPOS2 9
#define MPOS1 8
#define MPOS2 7
void setup()
{
pinMode(MOTORPWM, OUTPUT);
pinMode(MOTORA, OUTPUT);
pinMode(MOTORB, OUTPUT);
pinMode(GPOS1, INPUT);
pinMode(GPOS2, INPUT);
pinMode(MPOS1, INPUT);
pinMode(MPOS2, INPUT);
digitalWrite(MOTORPWM, LOW);
digitalWrite(MOTORA, LOW);
digitalWrite(MOTORB, LOW);
Serial.begin(115200);
attachInterrupt(digitalPinToInterrupt(GPOS1), handle_gpos_change, CHANGE);
attachInterrupt(digitalPinToInterrupt(GPOS2), handle_gpos_change, CHANGE);
attachInterrupt(digitalPinToInterrupt(MPOS1), handle_mpos_change, CHANGE);
attachInterrupt(digitalPinToInterrupt(MPOS2), handle_mpos_change, CHANGE);
}
void printSigned(double x)
{
if (x >= 0)
Serial.print("+");
Serial.print(x);
}
void get_gpos(int *ppos1, int *ppos2)
{
*ppos1 = digitalRead(GPOS1);
*ppos2 = digitalRead(GPOS2);
}
void get_mpos(int *ppos1, int *ppos2)
{
*ppos1 = digitalRead(MPOS1);
*ppos2 = digitalRead(MPOS2);
}
int enc(int pos1, int pos2)
{
if (pos1 == 0 && pos2 == 0)
return 0;
if (pos1 == 0 && pos2 == 1)
return 1;
if (pos1 == 1 && pos2 == 1)
return 2;
if (pos1 == 1 && pos2 == 0)
return 3;
}
volatile int gposition = 0;
volatile int gpos1 = -1, gpos2 = -1;
void monitor_motor()
{
int pos1, pos2;
get_gpos(&pos1, &pos2);
if (pos1 != gpos1 || pos2 != gpos2)
{
int old_enc = enc(gpos1, gpos2);
int new_enc = enc(pos1, pos2);
if (old_enc != -1 && new_enc != old_enc)
{
if (new_enc == (old_enc + 1) % 4)
{
--gposition;
}
else if (new_enc == (old_enc + 3) % 4)
{
++gposition;
}
else
{
Serial.print("skipped motor encoding ");
Serial.print(old_enc);
Serial.print(" -> ");
Serial.print(new_enc);
Serial.print("\n");
}
}
gpos1 = pos1;
gpos2 = pos2;
}
}
volatile int mposition = 0;
volatile int mpos1 = -1, mpos2 = -1;
void monitor_mill()
{
int pos1, pos2;
get_mpos(&pos1, &pos2);
if (pos1 != mpos1 || pos2 != mpos2)
{
int old_enc = enc(mpos1, mpos2);
int new_enc = enc(pos1, pos2);
if (old_enc != -1 && new_enc != old_enc)
{
if (new_enc == (old_enc + 1) % 4)
{
++mposition;
}
else if (new_enc == (old_enc + 3) % 4)
{
--mposition;
}
else
{
Serial.print("skipped milling encoding ");
Serial.print(old_enc);
Serial.print(" -> ");
Serial.print(new_enc);
Serial.print("\n");
}
}
mpos1 = pos1;
mpos2 = pos2;
}
}
void handle_gpos_change()
{
monitor_motor();
}
void handle_mpos_change()
{
monitor_mill();
}
void set_speed(double speed)
{
int dir = (speed >= 0);
double s = speed;
if (s < 0)
s = -s;
int v = (int)(255 * s);
digitalWrite(MOTORB, dir ? HIGH : LOW);
digitalWrite(MOTORA, dir ? LOW : HIGH);
analogWrite(MOTORPWM, v);
}
void run_at_speed(int dir, double speed, int duration)
{
Serial.print(gposition);
Serial.print("\n");
set_speed(dir ? speed : -speed);
delayMicroseconds(duration);
digitalWrite(MOTORA, LOW);
digitalWrite(MOTORB, LOW);
}
void test_motor()
{
while (1)
{
for (int dir = 0; dir < 2; ++dir)
{
for (double speed = 0; speed <= 1.0; speed += 0.01)
run_at_speed(dir, speed, 100000);
run_at_speed(dir, 1.0, 1000000);
for (double speed = 1.0; speed >= 0; speed -= 0.01)
run_at_speed(dir, speed, 100000);
}
}
}
void test_mill_pos()
{
while (1)
{
Serial.print(mpos1);
Serial.print(",");
Serial.print(mpos2);
Serial.print(" ");
Serial.print(mposition);
Serial.print("\n");
delay(100);
}
}
/* PID coeffs */
/* crappy
#define KP 0.001
#define KI 0.0002
#define KD 0.001
*/
/* good */
/*
#define KP 0.05
#define KI 0.0005
#define KD 0
*/
/* less overshoot */
/*
#define KP 0.05
#define KI 0.0005
#define KD 0.001
*/
/* WITH MAX=2.5 */
/* overshoot 43, final error 1 */
/*
#define KP 0.05
#define KI 0.002
#define KD 0
*/
/* overshoot 7, final error <1 */
/*
#define KP 0.05
#define KI 0.002
#define KD 0.001
*/
/* more aggressive, tuned after final assembly */
#define KP 0.05
#define KI 0.02
#define KD 0
void run_pid()
{
double tlast = 0;
double sum_error = 0;
double elast = 0;
double spindle_pos_last = 0;
/* PID */
while (1)
{
delayMicroseconds(10000);
double t = micros() * 1e-6;
//double target_pos = 1000 + t * 500;
double spindle_pos = mposition / MREV;
double target_pos = (GREV * spindle_pos) / NTEETH;
double error = target_pos - gposition;
double dt = t - tlast;
double derror = (error - elast) / dt;
sum_error += error * dt;
tlast = t;
elast = error;
double v = KP * error + KI * sum_error + KD * derror;
double rpm = (spindle_pos - spindle_pos_last) * 60 / dt;
spindle_pos_last = spindle_pos;
if (v > M)
v = M;
if (v < -M)
v = -M;
set_speed(-v * 0.4);
Serial.print("t=");
Serial.print(t);
Serial.print(" rpm=");
Serial.print(rpm);
Serial.print(" p=");
Serial.print(gposition);
Serial.print(" tgt=");
Serial.print(target_pos);
Serial.print(" error=");
printSigned(error);
Serial.print(" P=");[ATTACH=full]104370[/ATTACH] [ATTACH=full]104371[/ATTACH] [ATTACH=full]104372[/ATTACH]
printSigned(KP * error);
Serial.print(" I=");
printSigned(KI * sum_error);
Serial.print(" D=");
printSigned(KD * derror);
Serial.print(" v=");
printSigned(v);
Serial.print("\n");
}
}
void loop()
{
//set_speed(1);
//while (1)
// ;
run_pid();
//test_mill_pos();
}
