You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
82 lines
2.7 KiB
82 lines
2.7 KiB
|
|
/*
|
|
This program uses an Arduino for a closed-loop control of a DC-motor.
|
|
Motor motion is detected by a quadrature encoder.
|
|
Two inputs named STEP and DIR allow changing the target position.
|
|
Serial port prints current position and target position every second.
|
|
Serial input can be used to feed a new location for the servo (no CR LF).
|
|
|
|
Pins used:
|
|
Digital inputs 2 & 8 are connected to the two encoder signals (AB).
|
|
Digital input 3 is the STEP input.
|
|
Analog input 0 is the DIR input.
|
|
Digital outputs 9 & 10 control the PWM outputs for the motor (I am using half L298 here).
|
|
|
|
|
|
Please note PID gains kp, ki, kd need to be tuned to each different setup.
|
|
*/
|
|
|
|
#include <PinChangeInt.h>
|
|
#include <PinChangeIntConfig.h>
|
|
#include <PID_v1.h>
|
|
#include <PID_AutoTune_v0.h>
|
|
#define encoder0PinA 2 // PD2;
|
|
#define encoder0PinB 8 // PC0;
|
|
#define M1 6
|
|
#define M2 5 // motor's PWM outputs
|
|
|
|
|
|
double kp=4,ki=100,kd=0.02;
|
|
double input=80, output=0, setpoint=180;
|
|
PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT);
|
|
volatile long encoder0Pos = 0;
|
|
|
|
long previousMillis = 0; // will store last time LED was updated
|
|
|
|
long target1=0; // destination location at any moment
|
|
|
|
//for motor control ramps 1.4
|
|
bool newStep = false;
|
|
bool oldStep = false;
|
|
bool dir = false;
|
|
|
|
void setup() {
|
|
pinMode(encoder0PinA, INPUT);
|
|
pinMode(encoder0PinB, INPUT);
|
|
PCintPort::attachInterrupt(encoder0PinB, doEncoderMotor0,CHANGE); // now with 4x resolution as we use the two edges of A & B pins
|
|
attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2
|
|
attachInterrupt(1, countStep , RISING); // step input on interrupt 1 - pin 3
|
|
TCCR1B = TCCR1B & 0b11111000 | 1; // set Hz PWM
|
|
Serial.begin (115200);
|
|
//Setup the pid
|
|
myPID.SetMode(AUTOMATIC);
|
|
myPID.SetSampleTime(1);
|
|
myPID.SetOutputLimits(-255,255);
|
|
}
|
|
|
|
void loop(){
|
|
input = encoder0Pos;
|
|
setpoint=target1;
|
|
myPID.Compute();
|
|
// interpret received data as an integer (no CR LR): provision for manual testing over the serial port
|
|
if(Serial.available()) target1=Serial.parseInt();
|
|
pwmOut(output);
|
|
// if(millis() % 3000 == 0) target1=random(2000); // that was for self test with no input from main controller
|
|
}
|
|
|
|
void pwmOut(int out) {
|
|
if(out<0) { analogWrite(M1,0); analogWrite(M2,abs(out)); }
|
|
else { analogWrite(M2,0); analogWrite(M1,abs(out)); }
|
|
}
|
|
|
|
const int QEM [16] = {0,-1,1,2,1,0,2,-1,-1,2,0,1,2,1,-1,0}; // Quadrature Encoder Matrix
|
|
static unsigned char New, Old;
|
|
void doEncoderMotor0(){
|
|
Old = New;
|
|
New = (PINB & 1 )+ ((PIND & 4) >> 1); //
|
|
encoder0Pos+= QEM [Old * 4 + New];
|
|
}
|
|
|
|
void countStep(){ if (PINC&B0000001) target1--;else target1++; } // pin A0 represents direction
|
|
|