Svenska ElektronikForumet

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--;
}

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; 

    
}

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"));
}