Lite Servoexperiment för skoj skull. Större motor testad.

[SeniorLemuren]

Testade att knåpa ihop 2 st Arduino Uno och en billig DC-motordriver (35 spänn). Tillsammans med en motor med vidhängande encoder, jag tagit ur en skrivare. Parametrarna för PID verkar stämma bra just för denna motor. Optimalt snabb inbromsning och acceleration.

Det är inte mycket oljud om man jämför med liknande stegmotordrivning. Skulle jag en gång bygga mig en liten fräs el. dyl. skulle jag absolut satsa på detta koncept. Fördelen är ju att man inte tappar steg. Samt att det är nästan helt tyst drivning. Det oväsen som hörs är biltrafiken ute på gatan.

Anledningen till att jag använder 2 st Arduino är att den ena tjänar enbart som pulsgivare (i stället för PC-n med ev fräsprogram.) till den andra Uno som agerar Servoenhet. Det är 2 portar in den ena är för rotation och den andra för antal pulser/tidsenhet.

En liten video där man kan höra ljudnivån:

ArduinoServo.jpg

[ViktorSigg]

Jag har funderat på det rätt länge varför inte någon med fräs och verktyg bygger en enkoder till standard 12V elmotor och börjar sälja mini servopaket. Jag skulle aldrig stå ut med oljudet från stegmotorer!

Borde gå att få till, säkert många med fräsar i BF30 storlek som gärna skulle köra servo istället.

[SeniorLemuren]

Varför inte. Men marknaden är väl inte så jättestor just i Sverige kanske. Ett annat problem är ju detta med CE-märkning m.m. Men man kan ju sälja byggsatser förståss.

[swapper]

Vore guld om man kunde få kolla på koden.
Försökte mig på att göra en servo av motorer jag hittade i en bandrobot, kvalitetsmotorer med bra encoders.
Dock skrek motorn ganska bra och jag fick inte till någon vettig PID.
Låter intressant att du kunnat skicka steg/riktning med.

[Lennart Aspenryd]

Bra grej där Senioren! För att veta vart man ska bör man veta vart man är! Och då givet riktning!

[SeniorLemuren]

Vore guld om man kunde få kolla på koden.
Försökte mig på att göra en servo av motorer jag hittade i en bandrobot, kvalitetsmotorer med bra encoders.
Dock skrek motorn ganska bra och jag fick inte till någon vettig PID.
Låter intressant att du kunnat skicka steg/riktning med.

PID biblioteket:

PIDLibrarymaster.zip

Huvudprogram Servo:

Kod: Markera allt

/*
   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 5 & 6 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 <PID_v1.h>
#define encoder0PinA  2 // PD2; 
#define encoder0PinB  8  // PB0;
#define M1            5
#define M2            6  // motor's PWM outputs


double kp=1.8,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;

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(2, INPUT);
  pinMode(encoder0PinA, INPUT); 
  pinMode(encoder0PinB, INPUT);  
  attachInterrupt(0, doEncoderMotor0, CHANGE);  // encoder pin on interrupt 0 - pin 2
  attachInterrupt(1, countStep, RISING);  //on pin 3
  
  Serial.begin (115200);

  //Setup the pid 
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(2);
  myPID.SetOutputLimits(-255,255);
} 

void loop(){
    input = encoder0Pos; 
    setpoint=target1;
    myPID.Compute();
    pwmOut(output); 
   // print encoder and target every second throgh the serial port 
    if(millis() > previousMillis+1000 )  {
    Serial.print(encoder0Pos);
    Serial.print("->");
    Serial.println(target1);
    previousMillis=millis();
    }
    // interpret received data as an integer (no CR LR)
    if(Serial.available()) target1=Serial.parseInt();
}

void pwmOut(int out) {
   if(out<0) { analogWrite(M1,0); analogWrite(M2,abs(out)); }
   else { analogWrite(M2,0); analogWrite(M1,abs(out)); }
  }


void doEncoderMotor0(){
  if (((PIND&B0000100)>>2) == HIGH) {   // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PB0
    if ((PINB&B0000001) == LOW) {  // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0
      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(){ // pin A0 represents direction
            if (PINC&B0000001) target1++;
            else target1--;
}

Enkelt testprogram som matar sevo för test:

Kod: Markera allt

// matar Servo med step
int var = 0;

int antRev1= 30;
int delay1 = 1000;

int antRev2= 100;
int delay2 = 100;



int paus1 = 300;
int paus2 = 1000;

void setup() {  
  pinMode(13, OUTPUT);//Step
  pinMode(12, OUTPUT);//Dir
  
}

void loop() {
  step();  
}

void step(){  
  digitalWrite(12, HIGH); 
  while(var < 100 * antRev1){
  digitalWrite(13, HIGH);  
  delayMicroseconds(delay1);          
  digitalWrite(13, LOW);    
  delayMicroseconds(delay1);            
  var=var+1;  
  }  
  delay(paus1);
  var=0;  
  digitalWrite(12, LOW);  
  while(var < 100 * antRev2){
  digitalWrite(13, HIGH);   
  delayMicroseconds(delay2);            
  digitalWrite(13, LOW);   
  delayMicroseconds(delay2);         
  var=var+1;
  }
  delay(paus2);  
  var=0; 

    
}

[Nemo86]

Hade en liknande idé i våras men inte tiden.
Hur många pulser hinner arduino att läsa? dvs rpm?

[SeniorLemuren]

Bra fråga. Skall kolla upp det vid tillfälle.

[SeniorLemuren]

Motorn snurrar max 440 r/m. Encodern har en upplösning av 2016 puls/varv (A och B tillsammans) = 887040 pulser min. Snabbare går inte med Uno. Men en encoder med 400 pulser/varv vore tillräckligt för t.ex ett fräsbygge.

Man skulle då teoretiskt kunna köra motorn i drygt 2200 r/m. Vill man köra snabbare kan man ju använda en Arduino Due eller en chipKIT Uno32 som är 5 ggr snabbare.

[ViktorSigg]

Teensy 3.2 (Cortex M) har hårdvarustöd för två 4x enkoder, men vet inte om det är implementerat i teensyduino. Teensy 2.0 kan köra mjuk enkoder med mer än 127 kHz. Har inte provat med 3.2 och mjuk enkoder men 3.2 är otroligt mycket snabbare än 2.0.

[SeniorLemuren]

Ja, Teensy 3.2 är ju en vass sak men tyvärr mer än dubbelt så dyr som Arduino DUE (cortex-m3). Men den kanske blir billigare med tiden.

[ViktorSigg]

Vart köper ni due om ni får den till halva priset mot teensy??

[Eli]

http://www.ebay.co.uk/itm/New-DUE-2012- ... SwzJ5XV7xn

[SeniorLemuren]

Denna köpte jag nyligen. Just nu billigast på ebay, inklusive frakt direkt i brevlådan pris 109.59.

Den billigaste Teensy 3.2 jag hittade var på Lawicel 219+ frakt 14kr

[SeniorLemuren]

@swapper. Fick du någon fart på din motor? Jag hittade en sketch där man kunde ändra P,I,och D via seriemonitorn i Arduino. Men lyckades inte få sketchen att fungera.

Jag kompletterade i stället den sketch som jag använder med möjligheten att ändra PID från seriemonitorn och även spara nya värden i Unos EEPROM. Där kan man nu leka med olika värden medan motorn är igång.

Kod: Markera allt

/*
   This program uses an Arduino for a closed-loop control of a DC-motor. 
   Motor motion is detected by a quadrature encoder.   
   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 5 & 6 control the PWM outputs for the motor.


   Please note PID gains kp, ki, kd need to be tuned to each different setup. 
*/
#include <EEPROM.h>
#include <PID_v1.h>
#define encoder0PinA  2 // PD2; 
#define encoder0PinB  8  // PB0;
#define M1            5
#define M2            6  // motor's PWM outputs

int p=0;
double kp=1.8,ki=300,kd=0.02; // Start with this PID as default.
double input=0, output=0, setpoint=0;
PID myPID(&input, &output, &setpoint,kp,ki,kd, DIRECT);
volatile long encoder0Pos = 0;
long target1=0;  // destination location at any moment
bool newStep = false;
bool oldStep = false;
bool dir = false;

void setup() { 
  pinMode(2, INPUT);
  pinMode(encoder0PinA, INPUT); 
  pinMode(encoder0PinB, INPUT);  
  attachInterrupt(0, doEncoderMotor0, CHANGE);  // encoder pin on interrupt 0 - pin 2
  attachInterrupt(1, countStep, RISING);  //on pin 3
  
  Serial.begin (115200);

  //Setup the pid 
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(2);
  myPID.SetOutputLimits(-255,255);
} 

void loop(){
    if(Serial.available()) process_line();
    input = encoder0Pos; 
    setpoint=target1;
    myPID.Compute();
    pwmOut(output);     
}

void process_line() {
 char cmd = Serial.read();
 if(cmd>'Z') cmd-=32;
 switch(cmd) {
  case 'P': kp=Serial.parseFloat(); myPID.SetTunings(kp,ki,kd); break;
  case 'D': kd=Serial.parseFloat(); myPID.SetTunings(kp,ki,kd); break;
  case 'I': ki=Serial.parseFloat(); myPID.SetTunings(kp,ki,kd); break;
  case '?': printPos(); break;
  case 'Q': Serial.print("P="); Serial.print(kp); Serial.print(" I="); Serial.print(ki); Serial.print(" D="); Serial.println(kd); break;
  case 'H': help(); break;
  case 'W': writetoEEPROM(); break;  
  case 'R': recoverPIDfromEEPROM() ; break;  
  case 'S': showPIDinEEPROM() ; break;  
 }
 while(Serial.read()!=10); // dump extra characters till LF is seen (you can use CRLF or just LF)
}



void pwmOut(int out) {
   if(out<0) { analogWrite(M1,0); analogWrite(M2,abs(out)); }
   else { analogWrite(M2,0); analogWrite(M1,abs(out)); }
  }


void doEncoderMotor0(){
  if (((PIND&B0000100)>>2) == HIGH) {   // found a low-to-high on channel A; if(digitalRead(encoderPinA)==HIGH){.... read PB0
    if ((PINB&B0000001) == LOW) {  // check channel B to see which way; if(digitalRead(encoderPinB)==LOW){.... read PB0
      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(){ // pin A0 represents direction
            if (PINC&B0000001) target1++;
            else target1--;
}
// Här är alla rutiner från komandoraden samlade

void help() { 
 Serial.println(F("Available serial commands: (lines end with CRLF or LF)"));
 Serial.println(F("P123.34 sets proportional term to 123.34"));
 Serial.println(F("I123.34 sets integral term to 123.34"));
 Serial.println(F("D123.34 sets derivative term to 123.34"));
 Serial.println(F("? prints out current encoder, output and setpoint values"));
 Serial.println(F("Q will print out the current values of P, I and D parameters")); 
 Serial.println(F("R will recover PID from EEPROM")); 
 Serial.println(F("W will store current values of P, I and D parameters into EEPROM")); 
 Serial.println(F("H will print this help message again")); 
 Serial.println(F("S will print out the actual EEPROM PID values")); 
 
}

void printPos() {
  Serial.print(F("Position=")); Serial.print(encoder0Pos); Serial.print(F(" PID_output=")); Serial.print(output); Serial.print(F(" Target=")); Serial.println(setpoint);
}

void writetoEEPROM() { // keep PID set values in EEPROM so they are kept when arduino goes off
  eeput(kp,0);
  eeput(ki,4);
  eeput(kd,8);
  double cks=0;
  for(int i=0; i<12; i++) cks+=EEPROM.read(i);
  eeput(cks,12);
  Serial.println("\nPID values stored to EEPROM");
  //Serial.println(cks);
}

void eeput(double value, int dir) { 
  char * addr = (char * ) &value;
  for(int i=dir; i<dir+4; i++)  EEPROM.write(i,addr[i-dir]);
}

double eeget(int dir) { 
  double value;
  char * addr = (char * ) &value;
  for(int i=dir; i<dir+4; i++) addr[i-dir]=EEPROM.read(i);
  return value;
}

void recoverPIDfromEEPROM() {
  double cks=0;
  double cksEE;
  for(int i=0; i<12; i++) cks+=EEPROM.read(i);
  cksEE=eeget(12);
  //Serial.println(cks);
  if(cks==cksEE) {
    Serial.println(F("*** New PID values set from EEPROM***"));
    kp=eeget(0);
    ki=eeget(4);
    kd=eeget(8);
    myPID.SetTunings(kp,ki,kd);
    Serial.print("p=");
    Serial.print(kp);
    Serial.print(" i=");
    Serial.print(ki);
    Serial.print(" k=");
    Serial.print(kd);
    
  } 
  else Serial.println(F("*** Bad checksum"));
}

void showPIDinEEPROM(){
  double cks=0;
  double cksEE;
  for(int i=0; i<12; i++) cks+=EEPROM.read(i);
  cksEE=eeget(12);
  //Serial.println(cks);
  if(cks==cksEE) {
    Serial.println(F("*** Actual PID values in EEPROM***"));
    kp=eeget(0);
    ki=eeget(4);
    kd=eeget(8);    
    Serial.print("p=");
    Serial.print(kp);
    Serial.print(" i=");
    Serial.print(ki);
    Serial.print(" k=");
    Serial.print(kd);
    
  } 
  else Serial.println(F("*** Bad checksum"));
}