diff --git a/Moteur DC/ServoStrap_V05/ServoStrap_V05/ServoStrap_V05.ino b/Moteur DC/ServoStrap_V05/ServoStrap_V05/ServoStrap_V05.ino new file mode 100644 index 0000000..d791f73 --- /dev/null +++ b/Moteur DC/ServoStrap_V05/ServoStrap_V05/ServoStrap_V05.ino @@ -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 +#include +#include +#include +#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 + diff --git a/Moteur DC/ServoStrap_V05/ServoStrap_V05_AUTOTUNE/ServoStrap_V05_AUTOTUNE.ino b/Moteur DC/ServoStrap_V05/ServoStrap_V05_AUTOTUNE/ServoStrap_V05_AUTOTUNE.ino new file mode 100644 index 0000000..4ac01a0 --- /dev/null +++ b/Moteur DC/ServoStrap_V05/ServoStrap_V05_AUTOTUNE/ServoStrap_V05_AUTOTUNE.ino @@ -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 +#include +#include +#include +#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 +