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

Driver_Moteur_DC première impression viable

Browse Source 
YATA :)
This commit is contained in:
Vincent MERIL
2016-03-03 02:06:03 +01:00
parent c9ba9f7ffb
commit 66ae35ea13
96 changed files with 458 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

191
Moteur DC/ServoStrap/OLD/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation.pde Executable file
View File

@@ -0,0 +1,191 @@
#include <digitalWriteFast.h>
/* i have made this code for the LMD18245 motor controller,
i have merged the pid code of Josh Kopel
whith the code of makerbot servo-controller board,
you can use this code on the some board changing some values.
Daniele Poddighe
external ardware require a quadrature encoder, timing slit strip and a dc motor,
all you can find inside an old printer, i have took it from canon and hp printers(psc1510)
for motor controll you can choose different type of H-bridge, i have used LMD18245,
you can order 3 of it on ti.com sample request, the hardware needed is explained on the datasheet but i'm drowing
the schematic and PCB layout on eagle.
read a rotary encoder with interrupts
Encoder hooked up with common to GROUND,
encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below)
it doesn't matter which encoder pin you use for A or B
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
*/
#define encoder0PinA 2 // PD2;
#define encoder0PinB 8 // PB0;
#define SpeedPin 6
#define DirectionPin 15 //PC1;
//from ramps 1.4 stepper driver
#define STEP_PIN 3 //PD3;
#define DIR_PIN 14 //PC0;
//#define ENABLE_PIN 13 //PB5; for now is USELESS
//to use current motor as speed control, the LMD18245 has 4 bit cuttent output
//#define M0 9 //assign 4 bit from PORTB register to current control -> Bxx0000x (x mean any)
//#define M1 10 // PB1; PB2; PB3; PB4
//#define M2 11
//#define M3 12
volatile long encoder0Pos = 0;
long target = 0;
long target1 = 0;
int amp=212;
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 6.0 ; //position multiplier (gain) 2.25
float KI = 0.1; // Intergral multiplier (gain) .25
float KD = 1.3; // derivative multiplier (gain) 1.0
int lastError = 0;
int sumError = 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(DirectionPin, OUTPUT);
//pinMode(SpeedPin, OUTPUT);
//ramps 1.4 motor control
pinModeFast(STEP_PIN, INPUT);
pinModeFast(DIR_PIN, INPUT);
//pinModeFast(M0,OUTPUT);
//pinModeFast(M1,OUTPUT);
//pinModeFast(M2,OUTPUT);
//pinModeFast(M3,OUTPUT);
attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2
attachInterrupt(1, countStep, RISING); //on pin 3
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);
}
if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time
Serial.print(encoder0Pos);
Serial.print(',');
Serial.println(target1);
previousTarget=millis();
}
target = target1;
docalc();
}
void docalc() {
if (millis() - previousMillis > interval)
{
previousMillis = millis(); // remember the last time we blinked the LED
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 ms = 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;
}
if(ms > 0){
PORTC |=B00000010; //digitalWriteFast2 ( DirectionPin ,HIGH ); write PC1 HIGH
}
if(ms < 0){
PORTC &=(B11111101); //digitalWriteFast2 ( DirectionPin , LOW ); write PC1 LOW
ms = -1 * ms;
}
int motorspeed = map(ms,0,amp,0,255);
if( motorspeed >= 255) motorspeed=255;
//PORTB |=(motorspeed<<1); // is a sort of: digitalwrite(M0 M1 M2 M3, 0 0 0 0 to 1 1 1 1); it set directly PORTB to B**M3M2M1M0*;
//analogWrite ( SpeedPin, (255 - motorSpeed) );
analogWrite ( SpeedPin, motorspeed );
//Serial.print ( ms );
//Serial.print ( ',' );
//Serial.println ( motorspeed );
}
}
void doEncoderMotor0(){
if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PB0
// because PD0is used for serial, i will change in the stable version TO USE PD2
if ((PINB&B0000001) == LOW) { // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0
// 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
// encoder is turning
encoder0Pos++ ; // CW
}
else {
encoder0Pos-- ; // CCW
}
}
}
void countStep(){
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 (dir) target1++;
else target1--;
}

175
Moteur DC/ServoStrap/OLD/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/STM32_olimexino_stepper_motor_emulator Executable file
View File

@@ -0,0 +1,175 @@
/* i have made this code for the LMD18245 motor controller,
i have merged the pid code of Josh Kopel
whith the code of makerbot servo-controller board,
you can use this code on the some board changing some values.
Daniele Poddighe
external ardware require a quadrature encoder, timing slit strip and a dc motor,
all you can find inside an old printer, i have took it from canon and hp printers(psc1510)
for motor controll you can choose different type of H-bridge, i have used LMD18245,
you can order 3 of it on ti.com sample request, the hardware needed is explained on the datasheet but i'm drowing
the schematic and PCB layout on eagle.
read a rotary encoder with interrupts
Encoder hooked up with common to GROUND,
encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below)
it doesn't matter which encoder pin you use for A or B
is possible to change PID costants by sending on SerialUSB 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
*/
#define encoder0PinA 2
#define encoder0PinB 4
#define SpeedPin 9
#define DirectionPin 8
//from ramps 1.4 stepper driver
#define STEP_PIN 3
#define DIR_PIN 12
#define ENABLE_PIN 13
volatile long encoder0Pos = 0;
long target = 0;
long target1 = 0;
int amp=212;
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 6.0 ; //position multiplier (gain) 2.25
float KI = 0.1; // Intergral multiplier (gain) .25
float KD = 1.3; // derivative multiplier (gain) 1.0
int lastError = 0;
int sumError = 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() {
pinMode(encoder0PinA, INPUT);
pinMode(encoder0PinB, INPUT);
pinMode(DirectionPin, OUTPUT);
pinMode(SpeedPin, OUTPUT);
//ramps 1.4 motor control
pinMode(STEP_PIN, INPUT);
pinMode(DIR_PIN, INPUT);
attachInterrupt(2, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2
attachInterrupt(3, countStep, RISING); //on pin 3
SerialUSB.println("start"); // a personal quirk
}
void loop(){
while (SerialUSB.available() > 0) {
KP = SerialUSB.read();
KD = SerialUSB.read();
KI = SerialUSB.read();
SerialUSB.println(KP);
SerialUSB.println(KD);
SerialUSB.println(KI);
}
if(millis() - previousTarget > 500){ //enable this code only for test purposes
SerialUSB.print(encoder0Pos);
SerialUSB.print(',');
SerialUSB.println(target1);
previousTarget=millis();
}
target = target1;
docalc();
}
void docalc() {
if (millis() - previousMillis > interval)
{
previousMillis = millis(); // remember the last time we blinked the LED
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 ms = 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;
}
if(ms > 0){
digitalWrite ( DirectionPin ,HIGH );
}
if(ms < 0){
digitalWrite ( DirectionPin , LOW );
ms = -1 * ms;
}
int motorspeed = map(ms,0,amp,0,255);
if( motorspeed >= 255) motorspeed=255;
//analogWrite ( SpeedPin, (255 - motorSpeed) );
analogWrite ( SpeedPin, motorspeed );
//SerialUSB.print ( ms );
//SerialUSB.print ( ',' );
//SerialUSB.println ( motorspeed );
}
}
void doEncoderMotor0(){
if (digitalRead(encoder0PinA) == HIGH) { // found a low-to-high on channel A
if (digitalRead(encoder0PinB) == LOW) { // check channel B to see which way
// encoder is turning
encoder0Pos = encoder0Pos - 1; // CCW
}
else {
encoder0Pos = encoder0Pos + 1; // CW
}
}
else // found a high-to-low on channel A
{
if (digitalRead(encoder0PinB) == LOW) { // check channel B to see which way
// encoder is turning
encoder0Pos = encoder0Pos + 1; // CW
}
else {
encoder0Pos = encoder0Pos - 1; // CCW
}
}
}
void countStep(){
dir = digitalRead(DIR_PIN);
if (dir) target1++;
else target1--;
}

20
Moteur DC/ServoStrap/OLD/Arduino_stepper_motor_emulator_v1.0.1_direct_port_manipulation/keywords.txt Executable file
View File

@@ -0,0 +1,20 @@
#######################################
# Syntax Coloring Map For DigitalWriteFast
#######################################
#######################################
# Datatypes (KEYWORD1)
#######################################
DigitalWriteFast KEYWORD1
#######################################
# Methods and Functions (KEYWORD2)
#######################################
digitalWriteFast KEYWORD2
digitalWriteFast2 KEYWORD2
pinModeFast KEYWORD2
pinModeFast2 KEYWORD2
digitalReadFast KEYWORD2
digitalReadFast2 KEYWORD2

192
Moteur DC/ServoStrap/OLD/Driver_printer/Driver_printer.ino Executable file
View File

@@ -0,0 +1,192 @@
#include <digitalWriteFast.h>
/* i have made this code for the LMD18245 motor controller,
i have merged the pid code of Josh Kopel
whith the code of makerbot servo-controller board,
you can use this code on the some board changing some values.
Daniele Poddighe
external ardware require a quadrature encoder, timing slit strip and a dc motor,
all you can find inside an old printer, i have took it from canon and hp printers(psc1510)
for motor controll you can choose different type of H-bridge, i have used LMD18245,
you can order 3 of it on ti.com sample request, the hardware needed is explained on the datasheet but i'm drowing
the schematic and PCB layout on eagle.
read a rotary encoder with interrupts
Encoder hooked up with common to GROUND,
encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below)
it doesn't matter which encoder pin you use for A or B
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
*/
#define encoder0PinA 7 // PD2;
#define encoder0PinB 8 // PB0;
#define SpeedPin 10
#define DirectionPin 9 //PC1;
//from ramps 1.4 stepper driver
#define STEP_PIN 3 //PD3;
#define DIR_PIN 5 //PC0;
//#define ENABLE_PIN 11 //PB5; for now is USELESS
//to use current motor as speed control, the LMD18245 has 4 bit cuttent output
//#define M0 9 //assign 4 bit from PORTB register to current control -> Bxx0000x (x mean any)
//#define M1 10 // PB1; PB2; PB3; PB4
//#define M2 11
//#define M3 12
volatile long encoder0Pos = 0;
long target = 0;
long target1 = 0;
int amp=212;
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 6.0 ; //position multiplier (gain) 2.25
float KI = 0.1; // Intergral multiplier (gain) .25
float KD = 1.3; // derivative multiplier (gain) 1.0
int lastError = 0;
int sumError = 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(DirectionPin, OUTPUT);
//pinMode(SpeedPin, OUTPUT);
//ramps 1.4 motor control
pinModeFast(STEP_PIN, INPUT);
pinModeFast(DIR_PIN, INPUT);
//pinModeFast(M0,OUTPUT);
//pinModeFast(M1,OUTPUT);
//pinModeFast(M2,OUTPUT);
//pinModeFast(M3,OUTPUT);
attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2
attachInterrupt(1, countStep, RISING); //on pin 3
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);
}
if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time
Serial.print(encoder0Pos);
Serial.print(',');
Serial.println(target1);
previousTarget=millis();
}
target = target1;
docalc();
}
void docalc() {
if (millis() - previousMillis > interval)
{
previousMillis = millis(); // remember the last time we blinked the LED
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 ms = 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;
}
if(ms > 0){
PORTC |=B00000010; //digitalWriteFast2 ( DirectionPin ,HIGH ); write PC1 HIGH
}
if(ms < 0){
PORTC &=(B11111101); //digitalWriteFast2 ( DirectionPin , LOW ); write PC1 LOW
ms = -1 * ms;
}
int motorspeed = map(ms,0,amp,0,255);
if( motorspeed >= 255) motorspeed=255;
//PORTB |=(motorspeed<<1); // is a sort of: digitalwrite(M0 M1 M2 M3, 0 0 0 0 to 1 1 1 1); it set directly PORTB to B**M3M2M1M0*;
//analogWrite ( SpeedPin, (255 - motorSpeed) );
analogWrite ( SpeedPin, motorspeed );
//Serial.print ( ms );
//Serial.print ( ',' );
//Serial.println ( motorspeed );
}
}
void doEncoderMotor0(){
if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PB0
// because PD0is used for serial, i will change in the stable version TO USE PD2
if ((PINB&B0000001) == LOW) { // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0
// 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
// encoder is turning
encoder0Pos++ ; // CW
}
else {
encoder0Pos-- ; // CCW
}
}
}
void countStep(){
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 (dir) target1++;
else target1--;
}

165
Moteur DC/ServoStrap/OLD/Driver_printer/libraries/digitalWriteFast.h Executable file
View File

@@ -0,0 +1,165 @@
#if !defined(digitalPinToPortReg)
#if !defined(__AVR_ATmega1280__)
// Standard Arduino Pins
#define digitalPinToPortReg(P) \
(((P) >= 0 && (P) <= 7) ? &PORTD : (((P) >= 8 && (P) <= 13) ? &PORTB : &PORTC))
#define digitalPinToDDRReg(P) \
(((P) >= 0 && (P) <= 7) ? &DDRD : (((P) >= 8 && (P) <= 13) ? &DDRB : &DDRC))
#define digitalPinToPINReg(P) \
(((P) >= 0 && (P) <= 7) ? &PIND : (((P) >= 8 && (P) <= 13) ? &PINB : &PINC))
#define digitalPinToBit(P) \
(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14))
#if defined(__AVR_ATmega8__)
// 3 PWM
#define digitalPinToTimer(P) \
(((P) == 9 || (P) == 10) ? &TCCR1A : (((P) == 11) ? &TCCR2 : 0))
#define digitalPinToTimerBit(P) \
(((P) == 9) ? COM1A1 : (((P) == 10) ? COM1B1 : COM21))
#else
// 6 PWM
#define digitalPinToTimer(P) \
(((P) == 6 || (P) == 5) ? &TCCR0A : \
(((P) == 9 || (P) == 10) ? &TCCR1A : \
(((P) == 11 || (P) == 3) ? &TCCR2A : 0)))
#define digitalPinToTimerBit(P) \
(((P) == 6) ? COM0A1 : (((P) == 5) ? COM0B1 : \
(((P) == 9) ? COM1A1 : (((P) == 10) ? COM1B1 : \
(((P) == 11) ? COM2A1 : COM2B1)))))
#endif
#else
// Arduino Mega Pins
#define digitalPinToPortReg(P) \
(((P) >= 22 && (P) <= 29) ? &PORTA : \
((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &PORTB : \
(((P) >= 30 && (P) <= 37) ? &PORTC : \
((((P) >= 18 && (P) <= 21) || (P) == 38) ? &PORTD : \
((((P) >= 0 && (P) <= 3) || (P) == 5) ? &PORTE : \
(((P) >= 54 && (P) <= 61) ? &PORTF : \
((((P) >= 39 && (P) <= 41) || (P) == 4) ? &PORTG : \
((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &PORTH : \
(((P) == 14 || (P) == 15) ? &PORTJ : \
(((P) >= 62 && (P) <= 69) ? &PORTK : &PORTL))))))))))
#define digitalPinToDDRReg(P) \
(((P) >= 22 && (P) <= 29) ? &DDRA : \
((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &DDRB : \
(((P) >= 30 && (P) <= 37) ? &DDRC : \
((((P) >= 18 && (P) <= 21) || (P) == 38) ? &DDRD : \
((((P) >= 0 && (P) <= 3) || (P) == 5) ? &DDRE : \
(((P) >= 54 && (P) <= 61) ? &DDRF : \
((((P) >= 39 && (P) <= 41) || (P) == 4) ? &DDRG : \
((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &DDRH : \
(((P) == 14 || (P) == 15) ? &DDRJ : \
(((P) >= 62 && (P) <= 69) ? &DDRK : &DDRL))))))))))
#define digitalPinToPINReg(P) \
(((P) >= 22 && (P) <= 29) ? &PINA : \
((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &PINB : \
(((P) >= 30 && (P) <= 37) ? &PINC : \
((((P) >= 18 && (P) <= 21) || (P) == 38) ? &PIND : \
((((P) >= 0 && (P) <= 3) || (P) == 5) ? &PINE : \
(((P) >= 54 && (P) <= 61) ? &PINF : \
((((P) >= 39 && (P) <= 41) || (P) == 4) ? &PING : \
((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &PINH : \
(((P) == 14 || (P) == 15) ? &PINJ : \
(((P) >= 62 && (P) <= 69) ? &PINK : &PINL))))))))))
#define digitalPinToBit(P) \
(((P) >= 7 && (P) <= 9) ? (P) - 3 : \
(((P) >= 10 && (P) <= 13) ? (P) - 6 : \
(((P) >= 22 && (P) <= 29) ? (P) - 22 : \
(((P) >= 30 && (P) <= 37) ? 37 - (P) : \
(((P) >= 39 && (P) <= 41) ? 41 - (P) : \
(((P) >= 42 && (P) <= 49) ? 49 - (P) : \
(((P) >= 50 && (P) <= 53) ? 53 - (P) : \
(((P) >= 54 && (P) <= 61) ? (P) - 54 : \
(((P) >= 62 && (P) <= 69) ? (P) - 62 : \
(((P) == 0 || (P) == 15 || (P) == 17 || (P) == 21) ? 0 : \
(((P) == 1 || (P) == 14 || (P) == 16 || (P) == 20) ? 1 : \
(((P) == 19) ? 2 : \
(((P) == 5 || (P) == 6 || (P) == 18) ? 3 : \
(((P) == 2) ? 4 : \
(((P) == 3 || (P) == 4) ? 5 : 7)))))))))))))))
// 15 PWM
#define digitalPinToTimer(P) \
(((P) == 13 || (P) == 4) ? &TCCR0A : \
(((P) == 11 || (P) == 12) ? &TCCR1A : \
(((P) == 10 || (P) == 9) ? &TCCR2A : \
(((P) == 5 || (P) == 2 || (P) == 3) ? &TCCR3A : \
(((P) == 6 || (P) == 7 || (P) == 8) ? &TCCR4A : \
(((P) == 46 || (P) == 45 || (P) == 44) ? &TCCR5A : 0))))))
#define digitalPinToTimerBit(P) \
(((P) == 13) ? COM0A1 : (((P) == 4) ? COM0B1 : \
(((P) == 11) ? COM1A1 : (((P) == 12) ? COM1B1 : \
(((P) == 10) ? COM2A1 : (((P) == 9) ? COM2B1 : \
(((P) == 5) ? COM3A1 : (((P) == 2) ? COM3B1 : (((P) == 3) ? COM3C1 : \
(((P) == 6) ? COM4A1 : (((P) == 7) ? COM4B1 : (((P) == 8) ? COM4C1 : \
(((P) == 46) ? COM5A1 : (((P) == 45) ? COM5B1 : COM5C1))))))))))))))
#endif
#endif
#if !defined(digitalWriteFast)
#define digitalWriteFast(P, V) \
if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \
if (digitalPinToTimer(P)) \
bitClear(*digitalPinToTimer(P), digitalPinToTimerBit(P)); \
bitWrite(*digitalPinToPortReg(P), digitalPinToBit(P), (V)); \
} else { \
digitalWrite((P), (V)); \
}
#endif
#if !defined(pinModeFast)
#define pinModeFast(P, V) \
if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \
bitWrite(*digitalPinToDDRReg(P), digitalPinToBit(P), (V)); \
} else { \
pinMode((P), (V)); \
}
#endif
#if !defined(digitalReadFast)
#define digitalReadFast(P) ( (int) __digitalReadFast__((P)) )
#define __digitalReadFast__(P ) \
(__builtin_constant_p(P) ) ? ( \
digitalPinToTimer(P) ? ( \
bitClear(*digitalPinToTimer(P), digitalPinToTimerBit(P)) , \
bitRead(*digitalPinToPINReg(P), digitalPinToBit(P))) : \
bitRead(*digitalPinToPINReg(P), digitalPinToBit(P))) : \
digitalRead((P))
#endif
#if !defined(digitalWriteFast2)
#define digitalWriteFast2(P, V) \
if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \
bitWrite(*digitalPinToPortReg(P), digitalPinToBit(P), (V)); \
} else { \
digitalWrite((P), (V)); \
}
#endif
#if !defined(pinModeFast2)
#define pinModeFast2(P, V) \
if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \
if (digitalPinToTimer(P)) \
bitClear(*digitalPinToTimer(P), digitalPinToTimerBit(P)); \
bitWrite(*digitalPinToDDRReg(P), digitalPinToBit(P), (V)); \
} else { \
pinMode((P), (V)); \
}
#endif
#if !defined(digitalReadFast2)
#define digitalReadFast2(P) ( (int) __digitalReadFast2__((P)) )
#define __digitalReadFast2__(P ) \
(__builtin_constant_p(P) ) ? ( \
( bitRead(*digitalPinToPINReg(P), digitalPinToBit(P))) ) : \
digitalRead((P))
#endif

203
Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.0.pde Executable file
View File

@@ -0,0 +1,203 @@
#include <digitalWriteFast.h>
//this is to use DWF library, it will increase the speed of digitalRead/Write command
//used in the interrupt function doEncoderMotor0, but may be used everywhere.
/*
https://github.com/danithebest91/ServoStrap
i have made this code for the LMD18245 motor controller,
i have merged the pid code of Josh Kopel
whith the code of makerbot servo-controller board,
you can use this code on the some board changing some values.
Daniele Poddighe
external ardware require a quadrature encoder, timing slit strip and a dc motor,
all you can find inside an old printer, i have took it from canon and hp printers(psc1510)
for motor controll you can choose different type of H-bridge, i have used LMD18245,
you can order 3 of it on ti.com sample request, the hardware needed is explained on the datasheet but i'm drowing
the schematic and PCB layout on eagle.
read a rotary encoder with interrupts
Encoder hooked up with common to GROUND,
encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below)
it doesn't matter which encoder pin you use for A or B
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
This code use Port manipulation :
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
https://www.arduino.cc/en/Hacking/PinMapping
https://www.arduino.cc/en/Hacking/PinMapping168
*/
#define encoder0PinA 2 // PD2 (INT0) You can't modify port mapping here because this code use Port manipulation (I2)
#define encoder0PinB 8 // PB0; (I8)
#define SpeedPin 6 (I6)
#define DirectionPin 15 //PC1; (A1)
//from ramps 1.4 stepper driver
#define STEP_PIN 3 //PD3 (INT1) (I3)
#define DIR_PIN 14 //PC0; (A0)
//#define ENABLE_PIN 13 //PB5; for now is USELESS (I13)
//to use current motor as speed control, the LMD18245 has 4 bit cuttent output
//#define M0 9 //assign 4 bit from PORTB register to current control -> Bxx0000x (x mean any)
//#define M1 10 // PB1; PB2; PB3; PB4
//#define M2 11
//#define M3 12
volatile long encoder0Pos = 0;
long target = 0;
long target1 = 0;
int amp=212;
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 6.0 ; //position multiplier (gain) 2.25
float KI = 0.1; // Intergral multiplier (gain) .25
float KD = 1.3; // derivative multiplier (gain) 1.0
int lastError = 0;
int sumError = 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(DirectionPin, OUTPUT);
//pinMode(SpeedPin, OUTPUT);
//ramps 1.4 motor control
pinModeFast(STEP_PIN, INPUT);
pinModeFast(DIR_PIN, INPUT);
//pinModeFast(M0,OUTPUT);
//pinModeFast(M1,OUTPUT);
//pinModeFast(M2,OUTPUT);
//pinModeFast(M3,OUTPUT);
attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2
attachInterrupt(1, countStep, RISING); //on pin 3
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);
}
if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time
Serial.print(encoder0Pos);
Serial.print(',');
Serial.println(target1);
previousTarget=millis();
}
target = target1;
docalc();
}
void docalc() {
if (millis() - previousMillis > interval)
{
previousMillis = millis(); // remember the last time we blinked the LED
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 ms = 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;
}
if(ms > 0){
PORTC |=B00000010; //digitalWriteFast2 ( DirectionPin ,HIGH ); write PC1 HIGH (A1)
}
if(ms < 0){
PORTC &=(B11111101); //digitalWriteFast2 ( DirectionPin , LOW ); write PC1 LOW (A1)
ms = -1 * ms;
}
int motorspeed = map(ms,0,amp,0,255);
if( motorspeed >= 255) motorspeed=255;
//PORTB |=(motorspeed<<1); // is a sort of: digitalwrite(M0 M1 M2 M3, 0 0 0 0 to 1 1 1 1); it set directly PORTB to B**M3M2M1M0*;
//analogWrite ( SpeedPin, (255 - motorSpeed) );
analogWrite ( SpeedPin, motorspeed );
//Serial.print ( ms );
//Serial.print ( ',' );
//Serial.println ( motorspeed );
}
}
void doEncoderMotor0(){
if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PD2 (I2)
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 = (PINC&B0000001); // dir=digitalRead(dir_pin) read PC0, 14 digital;
//here will be (PINB&B0000001) to not use shift in the stable version
if (dir) target1++;
else target1--;
}

202
Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1AVEC_ENA.ino Executable file
View File

@@ -0,0 +1,202 @@
#include <digitalWriteFast.h>
//this is to use DWF library, it will increase the speed of digitalRead/Write command
//used in the interrupt function doEncoderMotor0, but may be used everywhere.
/*
https://github.com/danithebest91/ServoStrap
i have made this code for the LMD18245 motor controller,
i have merged the pid code of Josh Kopel
whith the code of makerbot servo-controller board,
you can use this code on the some board changing some values.
Daniele Poddighe
external ardware require a quadrature encoder, timing slit strip and a dc motor,
all you can find inside an old printer, i have took it from canon and hp printers(psc1510)
for motor controll you can choose different type of H-bridge, i have used LMD18245,
you can order 3 of it on ti.com sample request, the hardware needed is explained on the datasheet but i'm drowing
the schematic and PCB layout on eagle.
read a rotary encoder with interrupts
Encoder hooked up with common to GROUND,
encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below)
it doesn't matter which encoder pin you use for A or B
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
This code use Port manipulation :
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
https://www.arduino.cc/en/Hacking/PinMapping
https://www.arduino.cc/en/Hacking/PinMapping168
*/
#define encoder0PinA 2 // PD2 (INT0) You can't modify port mapping here because this code use Port manipulation (I2)
#define encoder0PinB 8 // PB0; (I8)
#define MotorIN1 5 //(I5) IN1
#define MotorIN2 6 //(I6) IN2
#define MotorENA 9 //(I9) ENA
//from ramps 1.4 stepper driver
#define STEP_PIN 3 //PD3 (INT1) (I3)
#define DIR_PIN 14 //PC0; (A0)
//#define ENABLE_PIN 13 //PB5; for now is USELESS (I13)
//to use current motor as speed control, the LMD18245 has 4 bit cuttent output
//#define M0 9 //assign 4 bit from PORTB register to current control -> Bxx0000x (x mean any)
//#define M1 10 // PB1; PB2; PB3; PB4
//#define M2 11
//#define M3 12
volatile long encoder0Pos = 0;
long target = 0;
long target1 = 0;
int amp=212;
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 6.0 ; //position multiplier (gain) 2.25
float KI = 0.1; // Intergral multiplier (gain) .25
float KD = 1.3; // derivative multiplier (gain) 1.0
int lastError = 0;
int sumError = 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(DirectionPin, OUTPUT);
//pinMode(SpeedPin, OUTPUT);
//ramps 1.4 motor control
pinModeFast(STEP_PIN, INPUT);
pinModeFast(DIR_PIN, INPUT);
//pinModeFast(M0,OUTPUT);
//pinModeFast(M1,OUTPUT);
//pinModeFast(M2,OUTPUT);
//pinModeFast(M3,OUTPUT);
attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2
attachInterrupt(1, countStep, RISING); //on pin 3
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);
}
if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time
Serial.print(encoder0Pos);
Serial.print(',');
Serial.println(target1);
previousTarget=millis();
}
target = target1;
docalc();
}
void docalc() {
if (millis() - previousMillis > interval)
{
previousMillis = millis(); // remember the last time we blinked the LED
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 ms = 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;
}
if(ms > 0){
digitalWriteFast ( MotorIN1 ,LOW );
digitalWriteFast ( MotorIN2 ,HIGH );
}
if(ms < 0){
digitalWriteFast ( MotorIN2 ,LOW );
digitalWriteFast ( MotorIN1 ,HIGH );
ms = -1 * ms;
}
int motorspeed = map(ms,0,amp,0,255);
if( motorspeed >= 255) motorspeed=255;
analogWrite ( MotorENA, motorspeed );
}
}
void doEncoderMotor0(){
if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PD2 (I2)
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 = (PINC&B0000001); // dir=digitalRead(dir_pin) read PC0, 14 digital;
//here will be (PINB&B0000001) to not use shift in the stable version
if (dir) target1++;
else target1--;
}

198
Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA.ino Executable file
View File

@@ -0,0 +1,198 @@
#include <digitalWriteFast.h>
//this is to use DWF library, it will increase the speed of digitalRead/Write command
//used in the interrupt function doEncoderMotor0, but may be used everywhere.
/*
https://github.com/danithebest91/ServoStrap
i have made this code for the LMD18245 motor controller,
i have merged the pid code of Josh Kopel
whith the code of makerbot servo-controller board,
you can use this code on the some board changing some values.
Daniele Poddighe
external ardware require a quadrature encoder, timing slit strip and a dc motor,
all you can find inside an old printer, i have took it from canon and hp printers(psc1510)
for motor controll you can choose different type of H-bridge, i have used LMD18245,
you can order 3 of it on ti.com sample request, the hardware needed is explained on the datasheet but i'm drowing
the schematic and PCB layout on eagle.
read a rotary encoder with interrupts
Encoder hooked up with common to GROUND,
encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below)
it doesn't matter which encoder pin you use for A or B
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
This code use Port manipulation :
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
https://www.arduino.cc/en/Hacking/PinMapping
https://www.arduino.cc/en/Hacking/PinMapping168
*/
#define encoder0PinA 2 // PD2 (INT0) You can't modify port mapping here because this code use Port manipulation (I2)
#define encoder0PinB 8 // PB0; (I8)
#define MotorIN1 5 //(I5) IN1
#define MotorIN2 6 //(I6) IN2
//from ramps 1.4 stepper driver
#define STEP_PIN 3 //PD3 (INT1) (I3)
#define DIR_PIN 14 //PC0; (A0)
//#define ENABLE_PIN 13 //PB5; for now is USELESS (I13)
//to use current motor as speed control, the LMD18245 has 4 bit cuttent output
//#define M0 9 //assign 4 bit from PORTB register to current control -> Bxx0000x (x mean any)
//#define M1 10 // PB1; PB2; PB3; PB4
//#define M2 11
//#define M3 12
volatile long encoder0Pos = 0;
long target = 0;
long target1 = 0;
int amp=212;
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 6.0 ; //position multiplier (gain) 2.25
float KI = 0.1; // Intergral multiplier (gain) .25
float KD = 1.3; // derivative multiplier (gain) 1.0
int lastError = 0;
int sumError = 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(DirectionPin, OUTPUT);
//pinMode(SpeedPin, OUTPUT);
//ramps 1.4 motor control
pinModeFast(STEP_PIN, INPUT);
pinModeFast(DIR_PIN, INPUT);
//pinModeFast(M0,OUTPUT);
//pinModeFast(M1,OUTPUT);
//pinModeFast(M2,OUTPUT);
//pinModeFast(M3,OUTPUT);
attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2
attachInterrupt(1, countStep, RISING); //on pin 3
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);
}
if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time
Serial.print(encoder0Pos);
Serial.print(',');
Serial.println(target1);
previousTarget=millis();
}
target = target1;
docalc();
}
void docalc() {
if (millis() - previousMillis > interval)
{
previousMillis = millis(); // remember the last time we blinked the LED
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 ms = 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;
}
int motorspeed = map(ms,0,amp,0,255);
if( motorspeed >= 255) motorspeed=255;
if(ms > 0){
digitalWriteFast ( MotorIN1 ,LOW );
analogWrite ( MotorIN2, motorspeed );
}
if(ms < 0){
digitalWriteFast ( MotorIN2 ,LOW );
analogWrite ( MotorIN1, motorspeed );
ms = -1 * ms;
}
}
}
void doEncoderMotor0(){
if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PD2 (I2)
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 = (PINC&B0000001); // dir=digitalRead(dir_pin) read PC0, 14 digital;
//here will be (PINB&B0000001) to not use shift in the stable version
if (dir) target1++;
else target1--;
}

200
Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2/ServoStrap_Port_manipulation_LD293D-1.1SS_ENA2.ino Executable file
View File

@@ -0,0 +1,200 @@
#include <digitalWriteFast.h>
//this is to use DWF library, it will increase the speed of digitalRead/Write command
//used in the interrupt function doEncoderMotor0, but may be used everywhere.
/*
https://github.com/danithebest91/ServoStrap
i have made this code for the LMD18245 motor controller,
i have merged the pid code of Josh Kopel
whith the code of makerbot servo-controller board,
you can use this code on the some board changing some values.
Daniele Poddighe
external ardware require a quadrature encoder, timing slit strip and a dc motor,
all you can find inside an old printer, i have took it from canon and hp printers(psc1510)
for motor controll you can choose different type of H-bridge, i have used LMD18245,
you can order 3 of it on ti.com sample request, the hardware needed is explained on the datasheet but i'm drowing
the schematic and PCB layout on eagle.
read a rotary encoder with interrupts
Encoder hooked up with common to GROUND,
encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below)
it doesn't matter which encoder pin you use for A or B
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
This code use Port manipulation :
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
https://www.arduino.cc/en/Hacking/PinMapping
https://www.arduino.cc/en/Hacking/PinMapping168
*/
#define encoder0PinA 2 // PD2 (INT0) You can't modify port mapping here because this code use Port manipulation (I2)
#define encoder0PinB 8 // PB0; (I8)
#define MotorIN1 5 //(I5) IN1
#define MotorIN2 6 //(I6) IN2
//from ramps 1.4 stepper driver
#define STEP_PIN 3 //PD3 (INT1) (I3)
#define DIR_PIN 14 //PC0; (A0)
//#define ENABLE_PIN 13 //PB5; for now is USELESS (I13)
//to use current motor as speed control, the LMD18245 has 4 bit cuttent output
//#define M0 9 //assign 4 bit from PORTB register to current control -> Bxx0000x (x mean any)
//#define M1 10 // PB1; PB2; PB3; PB4
//#define M2 11
//#define M3 12
volatile long encoder0Pos = 0;
long target = 0;
long target1 = 0;
int amp=212;
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 6.0 ; //position multiplier (gain) 2.25
float KI = 0.1; // Intergral multiplier (gain) .25
float KD = 1.3; // derivative multiplier (gain) 1.0
int lastError = 0;
int sumError = 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(MotorIN1, OUTPUT);
pinModeFast(MotorIN2, OUTPUT);
//pinModeFast(DirectionPin, OUTPUT);
//pinMode(SpeedPin, OUTPUT);
//ramps 1.4 motor control
pinModeFast(STEP_PIN, INPUT);
pinModeFast(DIR_PIN, INPUT);
//pinModeFast(M0,OUTPUT);
//pinModeFast(M1,OUTPUT);
//pinModeFast(M2,OUTPUT);
//pinModeFast(M3,OUTPUT);
attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2
attachInterrupt(1, countStep, RISING); //on pin 3
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);
}
if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time
Serial.print(encoder0Pos);
Serial.print(',');
Serial.println(target1);
previousTarget=millis();
}
target = target1;
docalc();
}
void docalc() {
if (millis() - previousMillis > interval)
{
previousMillis = millis(); // remember the last time we blinked the LED
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 ms = 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;
}
int motorspeed = map(ms,0,amp,0,255);
if( motorspeed >= 255) motorspeed=255;
if(ms > 0){
digitalWrite ( MotorIN1 , LOW );
analogWrite ( MotorIN2, motorspeed );
}
if(ms < 0){
digitalWrite ( MotorIN1 , HIGH );
analogWrite ( MotorIN2, 255 - motorspeed );
ms = -1 * ms;
}
}
}
void doEncoderMotor0(){
if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PD2 (I2)
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 = (PINC&B0000001); // dir=digitalRead(dir_pin) read PC0, 14 digital;
//here will be (PINB&B0000001) to not use shift in the stable version
if (dir) target1++;
else target1--;
}

204
Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/ServoStrap_Port_manipulation_LD293D_1_0_pde.ino Executable file
View File

@@ -0,0 +1,204 @@
#include <digitalWriteFast.h>
//this is to use DWF library, it will increase the speed of digitalRead/Write command
//used in the interrupt function doEncoderMotor0, but may be used everywhere.
/*
https://github.com/danithebest91/ServoStrap
i have made this code for the LMD18245 motor controller,
i have merged the pid code of Josh Kopel
whith the code of makerbot servo-controller board,
you can use this code on the some board changing some values.
Daniele Poddighe
external ardware require a quadrature encoder, timing slit strip and a dc motor,
all you can find inside an old printer, i have took it from canon and hp printers(psc1510)
for motor controll you can choose different type of H-bridge, i have used LMD18245,
you can order 3 of it on ti.com sample request, the hardware needed is explained on the datasheet but i'm drowing
the schematic and PCB layout on eagle.
read a rotary encoder with interrupts
Encoder hooked up with common to GROUND,
encoder0PinA to pin 2, encoder0PinB to pin 4 (or pin 3 see below)
it doesn't matter which encoder pin you use for A or B
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
This code use Port manipulation :
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
https://www.arduino.cc/en/Hacking/PinMapping
https://www.arduino.cc/en/Hacking/PinMapping168
*/
#define encoder0PinA 2 // PD2 (INT0) You can't modify port mapping here because this code use Port manipulation (I2)
#define encoder0PinB 8 // PB0; (I8)
#define SpeedPin 6 //(I6)
#define DirectionPin 15 //PC1; (A1)
//from ramps 1.4 stepper driver
#define STEP_PIN 3 //PD3 (INT1) (I3)
#define DIR_PIN 14 //PC0; (A0)
//#define ENABLE_PIN 13 //PB5; for now is USELESS (I13)
//to use current motor as speed control, the LMD18245 has 4 bit cuttent output
//#define M0 9 //assign 4 bit from PORTB register to current control -> Bxx0000x (x mean any)
//#define M1 10 // PB1; PB2; PB3; PB4
//#define M2 11
//#define M3 12
volatile long encoder0Pos = 0;
long target = 0;
long target1 = 0;
int amp=212;
//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
//PID controller constants
float KP = 6.0 ; //position multiplier (gain) 2.25
float KI = 0.1; // Intergral multiplier (gain) .25
float KD = 1.3; // derivative multiplier (gain) 1.0
int lastError = 0;
int sumError = 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(DirectionPin, OUTPUT);
//pinMode(SpeedPin, OUTPUT);
//ramps 1.4 motor control
pinModeFast(STEP_PIN, INPUT);
pinModeFast(DIR_PIN, INPUT);
//pinModeFast(M0,OUTPUT);
//pinModeFast(M1,OUTPUT);
//pinModeFast(M2,OUTPUT);
//pinModeFast(M3,OUTPUT);
attachInterrupt(0, doEncoderMotor0, CHANGE); // encoder pin on interrupt 0 - pin 2
attachInterrupt(1, countStep, RISING); //on pin 3
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);
}
if(millis() - previousTarget > 1000){ //enable this code only for test purposes because it loss a lot of time
Serial.print(encoder0Pos);
Serial.print(',');
Serial.println(target1);
previousTarget=millis();
}
target = target1;
docalc();
}
void docalc() {
if (millis() - previousMillis > interval)
{
previousMillis = millis(); // remember the last time we blinked the LED
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 ms = 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;
}
if(ms > 0){
PORTC |=B00000010; //digitalWriteFast2 ( DirectionPin ,HIGH ); write PC1 HIGH (A1)
}
if(ms < 0){
PORTC &=(B11111101); //digitalWriteFast2 ( DirectionPin , LOW ); write PC1 LOW (A1)
ms = -1 * ms;
}
int motorspeed = map(ms,0,amp,0,255);
if( motorspeed >= 255) motorspeed=255;
//PORTB |=(motorspeed<<1); // is a sort of: digitalwrite(M0 M1 M2 M3, 0 0 0 0 to 1 1 1 1); it set directly PORTB to B**M3M2M1M0*;
//analogWrite ( SpeedPin, (255 - motorSpeed) );
analogWrite ( SpeedPin, motorspeed );
//Serial.print ( ms );
//Serial.print ( ',' );
//Serial.println ( motorspeed );
}
}
void doEncoderMotor0(){
if (((PIND&B0000100)>>2) == HIGH) { // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PD2 (I2)
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 = (PINC&B0000001); // dir=digitalRead(dir_pin) read PC0, 14 digital;
//here will be (PINB&B0000001) to not use shift in the stable version
if (dir) target1++;
else target1--;
}

165
Moteur DC/ServoStrap/OLD/ServoStrap_Port_manipulation_LD293D/digitalWriteFast.h Executable file
View File

@@ -0,0 +1,165 @@
#if !defined(digitalPinToPortReg)
#if !defined(__AVR_ATmega1280__)
// Standard Arduino Pins
#define digitalPinToPortReg(P) \
(((P) >= 0 && (P) <= 7) ? &PORTD : (((P) >= 8 && (P) <= 13) ? &PORTB : &PORTC))
#define digitalPinToDDRReg(P) \
(((P) >= 0 && (P) <= 7) ? &DDRD : (((P) >= 8 && (P) <= 13) ? &DDRB : &DDRC))
#define digitalPinToPINReg(P) \
(((P) >= 0 && (P) <= 7) ? &PIND : (((P) >= 8 && (P) <= 13) ? &PINB : &PINC))
#define digitalPinToBit(P) \
(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14))
#if defined(__AVR_ATmega8__)
// 3 PWM
#define digitalPinToTimer(P) \
(((P) == 9 || (P) == 10) ? &TCCR1A : (((P) == 11) ? &TCCR2 : 0))
#define digitalPinToTimerBit(P) \
(((P) == 9) ? COM1A1 : (((P) == 10) ? COM1B1 : COM21))
#else
// 6 PWM
#define digitalPinToTimer(P) \
(((P) == 6 || (P) == 5) ? &TCCR0A : \
(((P) == 9 || (P) == 10) ? &TCCR1A : \
(((P) == 11 || (P) == 3) ? &TCCR2A : 0)))
#define digitalPinToTimerBit(P) \
(((P) == 6) ? COM0A1 : (((P) == 5) ? COM0B1 : \
(((P) == 9) ? COM1A1 : (((P) == 10) ? COM1B1 : \
(((P) == 11) ? COM2A1 : COM2B1)))))
#endif
#else
// Arduino Mega Pins
#define digitalPinToPortReg(P) \
(((P) >= 22 && (P) <= 29) ? &PORTA : \
((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &PORTB : \
(((P) >= 30 && (P) <= 37) ? &PORTC : \
((((P) >= 18 && (P) <= 21) || (P) == 38) ? &PORTD : \
((((P) >= 0 && (P) <= 3) || (P) == 5) ? &PORTE : \
(((P) >= 54 && (P) <= 61) ? &PORTF : \
((((P) >= 39 && (P) <= 41) || (P) == 4) ? &PORTG : \
((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &PORTH : \
(((P) == 14 || (P) == 15) ? &PORTJ : \
(((P) >= 62 && (P) <= 69) ? &PORTK : &PORTL))))))))))
#define digitalPinToDDRReg(P) \
(((P) >= 22 && (P) <= 29) ? &DDRA : \
((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &DDRB : \
(((P) >= 30 && (P) <= 37) ? &DDRC : \
((((P) >= 18 && (P) <= 21) || (P) == 38) ? &DDRD : \
((((P) >= 0 && (P) <= 3) || (P) == 5) ? &DDRE : \
(((P) >= 54 && (P) <= 61) ? &DDRF : \
((((P) >= 39 && (P) <= 41) || (P) == 4) ? &DDRG : \
((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &DDRH : \
(((P) == 14 || (P) == 15) ? &DDRJ : \
(((P) >= 62 && (P) <= 69) ? &DDRK : &DDRL))))))))))
#define digitalPinToPINReg(P) \
(((P) >= 22 && (P) <= 29) ? &PINA : \
((((P) >= 10 && (P) <= 13) || ((P) >= 50 && (P) <= 53)) ? &PINB : \
(((P) >= 30 && (P) <= 37) ? &PINC : \
((((P) >= 18 && (P) <= 21) || (P) == 38) ? &PIND : \
((((P) >= 0 && (P) <= 3) || (P) == 5) ? &PINE : \
(((P) >= 54 && (P) <= 61) ? &PINF : \
((((P) >= 39 && (P) <= 41) || (P) == 4) ? &PING : \
((((P) >= 6 && (P) <= 9) || (P) == 16 || (P) == 17) ? &PINH : \
(((P) == 14 || (P) == 15) ? &PINJ : \
(((P) >= 62 && (P) <= 69) ? &PINK : &PINL))))))))))
#define digitalPinToBit(P) \
(((P) >= 7 && (P) <= 9) ? (P) - 3 : \
(((P) >= 10 && (P) <= 13) ? (P) - 6 : \
(((P) >= 22 && (P) <= 29) ? (P) - 22 : \
(((P) >= 30 && (P) <= 37) ? 37 - (P) : \
(((P) >= 39 && (P) <= 41) ? 41 - (P) : \
(((P) >= 42 && (P) <= 49) ? 49 - (P) : \
(((P) >= 50 && (P) <= 53) ? 53 - (P) : \
(((P) >= 54 && (P) <= 61) ? (P) - 54 : \
(((P) >= 62 && (P) <= 69) ? (P) - 62 : \
(((P) == 0 || (P) == 15 || (P) == 17 || (P) == 21) ? 0 : \
(((P) == 1 || (P) == 14 || (P) == 16 || (P) == 20) ? 1 : \
(((P) == 19) ? 2 : \
(((P) == 5 || (P) == 6 || (P) == 18) ? 3 : \
(((P) == 2) ? 4 : \
(((P) == 3 || (P) == 4) ? 5 : 7)))))))))))))))
// 15 PWM
#define digitalPinToTimer(P) \
(((P) == 13 || (P) == 4) ? &TCCR0A : \
(((P) == 11 || (P) == 12) ? &TCCR1A : \
(((P) == 10 || (P) == 9) ? &TCCR2A : \
(((P) == 5 || (P) == 2 || (P) == 3) ? &TCCR3A : \
(((P) == 6 || (P) == 7 || (P) == 8) ? &TCCR4A : \
(((P) == 46 || (P) == 45 || (P) == 44) ? &TCCR5A : 0))))))
#define digitalPinToTimerBit(P) \
(((P) == 13) ? COM0A1 : (((P) == 4) ? COM0B1 : \
(((P) == 11) ? COM1A1 : (((P) == 12) ? COM1B1 : \
(((P) == 10) ? COM2A1 : (((P) == 9) ? COM2B1 : \
(((P) == 5) ? COM3A1 : (((P) == 2) ? COM3B1 : (((P) == 3) ? COM3C1 : \
(((P) == 6) ? COM4A1 : (((P) == 7) ? COM4B1 : (((P) == 8) ? COM4C1 : \
(((P) == 46) ? COM5A1 : (((P) == 45) ? COM5B1 : COM5C1))))))))))))))
#endif
#endif
#if !defined(digitalWriteFast)
#define digitalWriteFast(P, V) \
if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \
if (digitalPinToTimer(P)) \
bitClear(*digitalPinToTimer(P), digitalPinToTimerBit(P)); \
bitWrite(*digitalPinToPortReg(P), digitalPinToBit(P), (V)); \
} else { \
digitalWrite((P), (V)); \
}
#endif
#if !defined(pinModeFast)
#define pinModeFast(P, V) \
if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \
bitWrite(*digitalPinToDDRReg(P), digitalPinToBit(P), (V)); \
} else { \
pinMode((P), (V)); \
}
#endif
#if !defined(digitalReadFast)
#define digitalReadFast(P) ( (int) __digitalReadFast__((P)) )
#define __digitalReadFast__(P ) \
(__builtin_constant_p(P) ) ? ( \
digitalPinToTimer(P) ? ( \
bitClear(*digitalPinToTimer(P), digitalPinToTimerBit(P)) , \
bitRead(*digitalPinToPINReg(P), digitalPinToBit(P))) : \
bitRead(*digitalPinToPINReg(P), digitalPinToBit(P))) : \
digitalRead((P))
#endif
#if !defined(digitalWriteFast2)
#define digitalWriteFast2(P, V) \
if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \
bitWrite(*digitalPinToPortReg(P), digitalPinToBit(P), (V)); \
} else { \
digitalWrite((P), (V)); \
}
#endif
#if !defined(pinModeFast2)
#define pinModeFast2(P, V) \
if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \
if (digitalPinToTimer(P)) \
bitClear(*digitalPinToTimer(P), digitalPinToTimerBit(P)); \
bitWrite(*digitalPinToDDRReg(P), digitalPinToBit(P), (V)); \
} else { \
pinMode((P), (V)); \
}
#endif
#if !defined(digitalReadFast2)
#define digitalReadFast2(P) ( (int) __digitalReadFast2__((P)) )
#define __digitalReadFast2__(P ) \
(__builtin_constant_p(P) ) ? ( \
( bitRead(*digitalPinToPINReg(P), digitalPinToBit(P))) ) : \
digitalRead((P))
#endif