nouvvelle version du code servostrap
This commit is contained in:
@@ -0,0 +1,246 @@
|
||||
#include <digitalWriteFast.h>
|
||||
|
||||
/*
|
||||
Fortement inspiré du projet :
|
||||
https://github.com/danithebest91/ServoStrap
|
||||
|
||||
is possible to change PID costants by sending on serial interfaces the values separated by ',' in this order: KP,KD,KI
|
||||
example: 5.2,3.1,0 so we have KP=5.2 KD=3.1 KI=0 is only for testing purposes, but i will leave this function with eeprom storage
|
||||
|
||||
Utilisation de la librairies digitalWriteFast qui permet d'utiliser la manipulation de port pour accélérer les lectures écritures de certaine entree/sortie :
|
||||
|
||||
Port registers allow for lower-level and faster manipulation of the i/o pins of the microcontroller on an Arduino board. The chips used on the Arduino board
|
||||
(the ATmega8 and ATmega168) have three ports:
|
||||
B (digital pin 8 to 13)
|
||||
C (analog input pins)
|
||||
D (digital pins 0 to 7)
|
||||
https://www.arduino.cc/en/Reference/PortManipulation
|
||||
|
||||
Petit rappel sur la puce arduino :
|
||||
https://www.arduino.cc/en/Hacking/PinMapping
|
||||
https://www.arduino.cc/en/Hacking/PinMapping168
|
||||
*/
|
||||
|
||||
// --------------- INPUT ---------------------------------------------------
|
||||
|
||||
// Les ports de l'encoder sont 2 pour bénéficier de l'interuption sur A et 8 pour utiliser le PortManipulation. Attention sur le port manipulation à ne pas utiliser les pin 9 à 13 !
|
||||
#define encoder0PinA 2 // PD2 (INT0)
|
||||
#define encoder0PinB 8 // PB0; (I8)
|
||||
|
||||
// Utilisation de 5 et 6 pour la commande moteur via le driver L293D. Le enable est connecté au vcc et le pwm est envoyé directement dans les input.
|
||||
#define MotorIN1 5 //(I5) IN1
|
||||
#define MotorIN2 6 //(I6) IN2
|
||||
|
||||
// Recuperation des step de la carte avec interuption sur le port 3 et PortManipulation sur le port A0. Attention sur le port manipulation à ne pas utiliser les pin A1 à A5 !
|
||||
#define STEP_PIN 3 //PD3 (INT1) (I3)
|
||||
#define DIR_PIN 14 //PC0; (A0)
|
||||
|
||||
// Récupération du fin de course x_min pour l'initialisation du moteur
|
||||
#define X_MIN 4 //(4)
|
||||
|
||||
// ------------------------------------------------------------------------
|
||||
|
||||
// ------------- CONSTANTE DU PID -------------
|
||||
|
||||
float KP = 3 ; //Porportionnel
|
||||
float KI = 2; // Intergrale
|
||||
float KD = 15; // derive
|
||||
|
||||
// ------------- CONSTANTE DU PID -------------
|
||||
|
||||
// Initialisation de la position encoder. Attention celle-ci est incrémenté via un CHANGE sur l'interuption 0. Le nombre de pas de la roue codeuse est donc double !
|
||||
volatile long encoder0Pos = 0;
|
||||
|
||||
long target = 0;
|
||||
long target1 = 0;
|
||||
int lastError = 0;
|
||||
int sumError = 0;
|
||||
int StartRoutine=0;
|
||||
|
||||
float OldTime = 0;
|
||||
float Oldencoder0Pos = 0;
|
||||
float MySpeed = 0;
|
||||
|
||||
//Integral term min/max (random value and not yet tested/verified)
|
||||
int iMax = 100;
|
||||
int iMin = 0;
|
||||
|
||||
long previousTarget = 0;
|
||||
long previousMillis = 0; // will store last time LED was updated
|
||||
long interval = 5; // interval at which to blink (milliseconds)
|
||||
|
||||
//for motor control ramps 1.4
|
||||
//bool newStep = false;
|
||||
//bool oldStep = false;
|
||||
//bool dir = false;
|
||||
|
||||
void setup() {
|
||||
pinModeFast(2, INPUT);
|
||||
pinModeFast(encoder0PinA, INPUT);
|
||||
pinModeFast(encoder0PinB, INPUT);
|
||||
pinModeFast(X_MIN, INPUT);
|
||||
|
||||
|
||||
pinModeFast(MotorIN1, OUTPUT);
|
||||
pinModeFast(MotorIN2, OUTPUT);
|
||||
|
||||
// Initailisation recuperation commande carte
|
||||
pinModeFast(STEP_PIN, INPUT);
|
||||
pinModeFast(DIR_PIN, INPUT);
|
||||
|
||||
// INTERUPTION
|
||||
|
||||
attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2
|
||||
attachInterrupt(1, countStep, RISING); //on pin 3
|
||||
|
||||
// INTERUPTION
|
||||
|
||||
Serial.begin (115200);
|
||||
Serial.println("start"); // a personal quirk
|
||||
|
||||
}
|
||||
|
||||
void loop(){
|
||||
|
||||
while (Serial.available() > 0) {
|
||||
KP = Serial.parseFloat();
|
||||
KD = Serial.parseFloat();
|
||||
KI = Serial.parseFloat();
|
||||
|
||||
|
||||
Serial.println(KP);
|
||||
Serial.println(KD);
|
||||
Serial.println(KI);
|
||||
}
|
||||
Fstart();
|
||||
if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time
|
||||
|
||||
// if (X_MIN == LOW) Serial.print("LOW");
|
||||
// else Serial.print("HIGH");
|
||||
// Serial.print(',');
|
||||
Serial.print(encoder0Pos);
|
||||
Serial.print(',');
|
||||
Serial.println(target1);
|
||||
previousTarget=millis();
|
||||
}
|
||||
|
||||
target = target1;
|
||||
docalc();
|
||||
}
|
||||
|
||||
void Fstart(){
|
||||
//Fonction start joue une seul fois
|
||||
// Devellopement de l'autotune en cours a intergrer au demarrage du moteur ainsi que le homing
|
||||
if (StartRoutine == 0)
|
||||
{
|
||||
digitalWrite ( MotorIN1 , LOW );
|
||||
analogWrite ( MotorIN2, 200 );
|
||||
delay(2000);
|
||||
digitalWrite ( MotorIN2 , LOW );
|
||||
analogWrite ( MotorIN1, 255 );
|
||||
delay(200);
|
||||
digitalWrite ( MotorIN1, LOW );
|
||||
analogWrite ( MotorIN2, 255 );
|
||||
delay(180);
|
||||
digitalWrite ( MotorIN2, LOW );
|
||||
analogWrite ( MotorIN1, 255 );
|
||||
delay(100);
|
||||
digitalWrite ( MotorIN1, LOW );
|
||||
analogWrite ( MotorIN2, 255 );
|
||||
delay(90);
|
||||
digitalWrite ( MotorIN2, LOW );
|
||||
analogWrite ( MotorIN1, 255 );
|
||||
delay(100);
|
||||
digitalWrite ( MotorIN1, LOW );
|
||||
analogWrite ( MotorIN2, 200 );
|
||||
delay(2000);
|
||||
|
||||
|
||||
StartRoutine = 1;
|
||||
encoder0Pos=0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Speed(){
|
||||
MySpeed = (Oldencoder0Pos - encoder0Pos) / ( OldTime - millis());
|
||||
OldTime = millis();
|
||||
Oldencoder0Pos = encoder0Pos;
|
||||
}
|
||||
|
||||
void docalc() {
|
||||
|
||||
if (millis() - previousMillis > interval)
|
||||
{
|
||||
previousMillis = millis();
|
||||
long error = encoder0Pos - target ; // find the error term of current position - target
|
||||
|
||||
//generalized PID formula
|
||||
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
|
||||
long motorspeed = KP * error + KD * (error - lastError) +KI * (sumError);
|
||||
|
||||
lastError = error;
|
||||
sumError += error;
|
||||
|
||||
//scale the sum for the integral term
|
||||
if(sumError > iMax) {
|
||||
sumError = iMax;
|
||||
} else if(sumError < iMin){
|
||||
sumError = iMin;
|
||||
}
|
||||
|
||||
Serial.println(motorspeed);
|
||||
|
||||
if(motorspeed > 0){
|
||||
if( motorspeed >= 255) motorspeed=255;
|
||||
digitalWrite ( MotorIN1 , LOW );
|
||||
analogWrite ( MotorIN2, motorspeed );
|
||||
}
|
||||
if(motorspeed < 0){
|
||||
motorspeed = -1 * motorspeed;
|
||||
if( motorspeed >= 255) motorspeed=255;
|
||||
digitalWrite ( MotorIN2 , LOW );
|
||||
analogWrite ( MotorIN1, motorspeed );
|
||||
//digitalWrite ( MotorIN1 , HIGH );
|
||||
//analogWrite ( MotorIN2, 255 - motorspeed );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void doEncoderMotor0(){
|
||||
//if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PD2 (I2)
|
||||
if(digitalRead(encoder0PinA)==HIGH){
|
||||
|
||||
if ((PINB&B0000001) == LOW) { // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0 (I8)
|
||||
// encoder is turning
|
||||
encoder0Pos-- ; // CCW
|
||||
}
|
||||
else {
|
||||
encoder0Pos++ ; // CW
|
||||
}
|
||||
}
|
||||
else // found a high-to-low on channel A
|
||||
{
|
||||
if ((PINB&B0000001) == LOW) { // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0 (I8)
|
||||
// encoder is turning
|
||||
encoder0Pos++ ; // CW
|
||||
}
|
||||
else {
|
||||
encoder0Pos-- ; // CCW
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void countStep(){
|
||||
//dir=digitalRead(DIR_PIN)==HIGH;
|
||||
//dir = (PINC&B0000001); // dir=digitalRead(dir_pin) read PC0, 14 digital;
|
||||
//here will be (PINB&B0000001) to not use shift in the stable version
|
||||
if (PINC&B0000001) {
|
||||
target1++;
|
||||
}
|
||||
else
|
||||
{
|
||||
target1--;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user