Meccano Créations

Code : Tout sélectionner

// https://github.com/curiores/ArduinoTutorials

//  for arduino nano

//  adapted to work with a two wire motor driver and to take input from a potentiometer.  StormingMoose 2024

#define ENCA 2// YELLOW
#define ENCB 1 // WHITE
#define PWRPIN 3         
#define DIRPIN 4
#define STEERINGPIN 5


volatile int posi = 0; // specify posi as volatile: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
long prevT = 0;
float eprev = 0;
float eintegral = 0;

void setup() {
  Serial.begin(9600);
  pinMode(ENCA,INPUT);
  pinMode(ENCB,INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCA),readEncoder,RISING);
  pinMode(PWRPIN,OUTPUT);
  //pinMode(IN1,OUTPUT);
  pinMode(DIRPIN,OUTPUT);
  
  Serial.println("target pos");
}

void loop() {

  // set target position
  //int target = 1200;
  int target = 200*sin(prevT/1e6);
 // int target = 3*analogRead(STEERINGPIN);

  // PID constants
  float kp = 1;
  float kd = 0.005;
  float ki = 0.0;

  // time difference
  long currT = micros();
  float deltaT = ((float) (currT - prevT))/( 1.0e6 );
  prevT = currT;

  // Read the position in an interrupt block to avoid a potential
  // misread if the interrupt coincides with this code running
  // see: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
  int pos = 0; 
  noInterrupts();
  //  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    pos = posi;
  //}
  interrupts();  
  
  
  // error
  int e = pos - target;

  // derivative
  float dedt = (e-eprev)/(deltaT);

  // integral
  eintegral = eintegral + e*deltaT;

  // control signal
  float u = kp*e + kd*dedt + ki*eintegral;

  // motor power
  float pwr = fabs(u);
  if( pwr > 255 ){
    pwr = 255;
  }

  // motor direction
  int dir = 1;
  if(u<0){
    dir = -1;
  }

  // signal the motor
  setMotor(dir,pwr,PWRPIN,DIRPIN);


  // store previous error
  eprev = e;

  Serial.print(target);
  Serial.print(" ");
  Serial.print(pos);
  Serial.println();
}

void setMotor(int dir, int pwr, int pwmpin, int dirpin){   
                                                          
  if(dir == 1){  // reverse                             
    digitalWrite(dirpin,HIGH);
    analogWrite(pwmpin,255-pwr);
  }
  else if(dir == -1){   // forward
    digitalWrite(dirpin,LOW);
    analogWrite(pwmpin,pwr);
  }
  else{
    digitalWrite(dirpin,LOW);
    analogWrite(pwmpin,0);
  }  
}

void readEncoder(){
  int b = digitalRead(ENCB);
  if(b > 0){
    posi++;
  }
  else{
    posi--;
  }
}