vmeril
/
3DPrinterMadeInPlateformeC
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Ajout ServoStrap V05 pour moteur DC

Browse Source
master
Vincent MERIL 9 years ago
parent 9cfe2ce3d7
commit 173237fc77
2 changed files with 263 additions and 0 deletions
Show Stats Download Patch File Download Diff File

81
Moteur DC/ServoStrap_V05/ServoStrap_V05/ServoStrap_V05.ino
Unescape View File

@ -0,0 +1,81 @@
/*
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

182
Moteur DC/ServoStrap_V05/ServoStrap_V05_AUTOTUNE/ServoStrap_V05_AUTOTUNE.ino
Unescape View File

@ -0,0 +1,182 @@
/*
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
byte ATuneModeRemember=2;
double kp=5,ki=300,kd=0.02;
double input=80, output=0, setpoint=180;
PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT);
volatile long encoder0Pos = 0;
double kpmodel=1.5, taup=100, theta[50];
double outputStart=5;
double aTuneStep=50, aTuneNoise=1, aTuneStartValue=100;
unsigned int aTuneLookBack=20;
boolean tuning = true;
unsigned long modelTime, serialTime;
PID_ATune aTune(&input, &output);
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() {
if(tuning)
{
tuning=false;
changeAutoTune();
tuning=true;
}
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
if(tuning)
{
byte val = (aTune.Runtime());
if (val!=0)
{
tuning = false;
}
if(!tuning)
{ //we're done, set the tuning parameters
kp = aTune.GetKp();
ki = aTune.GetKi();
kd = aTune.GetKd();
Serial.print("kp: ");Serial.print(myPID.GetKp());Serial.print(" ");
Serial.print("ki: ");Serial.print(myPID.GetKi());Serial.print(" ");
Serial.print("kd: ");Serial.print(myPID.GetKd());Serial.println();
myPID.SetTunings(kp,ki,kd);
AutoTuneHelper(false);
}
}
//send-receive with processing if it's time
if(millis()>serialTime)
{
SerialReceive();
SerialSend();
serialTime+=500;
}
}
void changeAutoTune()
{
if(!tuning)
{
//Set the output to the desired starting frequency.
output=aTuneStartValue;
aTune.SetNoiseBand(aTuneNoise);
aTune.SetOutputStep(aTuneStep);
aTune.SetLookbackSec((int)aTuneLookBack);
AutoTuneHelper(true);
tuning = true;
}
else
{ //cancel autotune
aTune.Cancel();
tuning = false;
AutoTuneHelper(false);
}
}
void AutoTuneHelper(boolean start)
{
if(start)
ATuneModeRemember = myPID.GetMode();
else
myPID.SetMode(ATuneModeRemember);
}
void SerialSend()
{
Serial.print("setpoint: ");Serial.print(setpoint); Serial.print(" ");
Serial.print("input: ");Serial.print(input); Serial.print(" ");
Serial.print("output: ");Serial.print(output); Serial.print(" ");
if(tuning){
Serial.println("tuning mode");
} else {
Serial.print("kp: ");Serial.print(myPID.GetKp());Serial.print(" ");
Serial.print("ki: ");Serial.print(myPID.GetKi());Serial.print(" ");
Serial.print("kd: ");Serial.print(myPID.GetKd());Serial.println();
}
}
void SerialReceive()
{
if(Serial.available())
{
char b = Serial.read();
Serial.flush();
if((b=='1' && !tuning) || (b!='1' && tuning))changeAutoTune();
}
}
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
Write Preview
Loading…
Cancel
Save