Driver_Moteur_DC première impression viable

YATA :)

Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_1/ServoStrapPortManipulationLD293D_1_1.ino

@@ -0,0 +1,229 @@
+#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 X_MIN               4 //(4)
+
+volatile long encoder0Pos = 0;
+
+long target = 0;
+long target1 = 0;
+int StartRoutine=0;
+//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
+//PID controller constants
+float KP = 2.25 ; //position multiplier (gain) 2.25
+float KI = 0.25; // Intergral multiplier (gain) .25
+float KD = 1.0; // 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(X_MIN, INPUT); 
+ 
+  
+   pinModeFast(MotorIN1, OUTPUT); 
+  pinModeFast(MotorIN2, OUTPUT); 
+  
+  //ramps 1.4 motor control
+    pinModeFast(STEP_PIN, INPUT);
+    pinModeFast(DIR_PIN, INPUT);
+
+  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);
+}
+  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(){
+
+    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 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;
+    }
+   
+    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 (dir) target1++;
+            else target1--;
+}

Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino

@@ -0,0 +1,229 @@
+#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 X_MIN               4 //(4)
+
+volatile long encoder0Pos = 0;
+
+long target = 0;
+long target1 = 0;
+int StartRoutine=0;
+//correction = Kp * error + Kd * (error - prevError) + kI * (sum of errors)
+//PID controller constants
+float KP = 5 ; //position multiplier (gain) 2.25
+float KI = 0.1; // Intergral multiplier (gain) .25
+float KD = 2.0; // 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(X_MIN, INPUT); 
+ 
+  
+   pinModeFast(MotorIN1, OUTPUT); 
+  pinModeFast(MotorIN2, OUTPUT); 
+  
+  //ramps 1.4 motor control
+    pinModeFast(STEP_PIN, INPUT);
+    pinModeFast(DIR_PIN, INPUT);
+
+  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);
+}
+  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(){
+
+    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 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;
+    }
+   
+    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 (dir) target1++;
+            else target1--;
+}