Go Down

Topic: Robot 2WD con encoder problemi codice (Read 125 times) previous topic - next topic

GeCo77

Salve a tutti, purtroppo sono nuovo e faccio molta fatica a programmare

il mio problema è questo:

ho il classico rover 2WD con motori DC (avrei voluto mettere una foto ma non ci sono riuscito se non in allegato.. forse..)
siccome nel farlo andare dritto in realtà andava storto (giustamente) quindi ho deciso di montare due encoder FC-03

per comandare i motori il solito L298N

e a gestire il tutto un arduino uno

cercando sul web ho trovato che
punto 1 - devo leggere gli encoder con la funzione interrupt
punto 2 - per far andare entrambe le ruote alla stessa velocità (rpm) conviene usare la libreria PID che vi allego di seguito

https://playground.arduino.cc/Code/PIDLibrary

ora però anche se ho scopiazzato in giro non riesco a far funzionare il tutto... se qualcuno vuole fare uno sguardo al codice:

Code: [Select]


#include <PID_v1.h>
 
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
 
//Specify the links and initial tuning parameters
PID myPID(&Input1, &Output, &Setpoint,2,5,1, DIRECT);
// Include the TimerOne Library from Paul Stoffregen
#include "TimerOne.h"
 
// Constants for Interrupt Pins
// Change values if not using Arduino Uno
 
const byte MOTOR1 = 2;  // Motor 1 Interrupt Pin - INT 0
const byte MOTOR2 = 3;  // Motor 2 Interrupt Pin - INT 1
 
// Integers for pulse counters
unsigned int counter1 = 0;
unsigned int counter2 = 0;


 
// Float for number of slots in encoder disk
float diskslots = 20;  // Change to match value of encoder disk
 
// Interrupt Service Routines
 
// Motor 1 pulse count ISR
void ISR_count1()  
{
  counter1++;  // increment Motor 1 counter value
}
 
// Motor 2 pulse count ISR
void ISR_count2()  
{
  counter2++;  // increment Motor 2 counter value
}
 
// TimerOne ISR
void ISR_timerone()
{
  Timer1.detachInterrupt();  // Stop the timer
  Serial.print("Motor Speed 1: ");
  float rotation1 = (counter1 / diskslots) * 60.00;  // calculate RPM for Motor 1
  Serial.print(rotation1);  
  Serial.print(" RPM - ");
  counter1 = 0;  //  reset counter to zero
  Serial.print("Motor Speed 2: ");
  float rotation2 = (counter2 / diskslots) * 60.00;  // calculate RPM for Motor 2
  Serial.print(rotation2);  
  Serial.println(" RPM");
  counter2 = 0;  //  reset counter to zero
  Timer1.attachInterrupt( ISR_timerone );  // Enable the timer
}
 
void setup()
{

/// Motor A
 
//int enA = 10;
int in1 = 9;
int in2 = 8;
 
// Motor B
 
//int enB = 5;
int in3 = 7;
int in4 = 6;
  
  Serial.begin(9600);
  
  Timer1.initialize(1000000); // set timer for 1sec
  attachInterrupt(digitalPinToInterrupt (MOTOR1), ISR_count1, RISING);  // Increase counter 1 when speed sensor pin goes High
  attachInterrupt(digitalPinToInterrupt (MOTOR2), ISR_count2, RISING);  // Increase counter 2 when speed sensor pin goes High
  Timer1.attachInterrupt( ISR_timerone ); // Enable the timer


   //initialize the variables we're linked to
  Input = (counter2 / diskslots) * 60.00;
  Setpoint = 1000;

    Input = (counter1 / diskslots) * 60.00;
  Setpoint = 1000;
 
  //turn the PID on
  myPID.SetMode(AUTOMATIC);
 
   // Set Motor A forward
   digitalWrite(in1, HIGH);
   digitalWrite(in2, LOW);
 
   // Set Motor B forward
   digitalWrite(in3, HIGH);
   digitalWrite(in4, LOW);
  
}
 
void loop()
{
  // Nothing in the loop!
  // You can place code here


  Input = (counter1 / diskslots) * 60.00;
  myPID.Compute();
  analogWrite(10,Output);

   Input = (counter2 / diskslots) * 60.00;
   myPID.Compute();
   analogWrite(5,Output);
 // Serial.print(Input); Serial.print(" "); Serial.println(Output);
}


i collegamenti sono i classici:

2 e 3 essendo gli unici interrupt sono collegati ai codec

6 e 7, e 8 e 9 sono per la direzione motori su L298N mentre 5 e 10 la velocità

scopo di tutto: vorrei "solo" che il rover andasse dritto.....



nid69ita

Ma cosa non ti funziona ? Non va dritto ? Non legge i sensori ? Non compila ?
my name is IGOR, not AIGOR

GeCo77

non compila

il programma mi legge bene gli rpm di entrambi i motori e li stampa su seriale.

non riesco a integrarci la funzione PID che deve gestire i motori in uscita su pin 5 e pin 10

ho problemi sia per la lettura PID imput (rpm) che output. inoltre non ho trovato da nessuna parte come usare due funzioni PID motore A e motore B.

GeCo77

provo a spiegarmi meglio... ho questo codice:

Code: [Select]

  Timer1.detachInterrupt();  // Stop the timer
  Serial.print("Motor Speed 1: ");
  float rotation1 = (counter1 / diskslots) * 60.00;  // calculate RPM for Motor 1
  Serial.print(rotation1); 
  Serial.print(" RPM - ");


che mi funziona benissimo stampandomi il dari rpm (rotatio1)

ora io dovrei mettere (rotation1) come input di questo comando:

Code: [Select]

  //initialize the variables we're linked to
  Input = analogRead(0);
  Setpoint = 100;

  //turn the PID on
  myPID.SetMode(AUTOMATIC);


quindi al posto di specificare il pin analogico dovrei fargli leggere il valore rotatio1... qualcuno sa come fare?

grazie

Go Up