jankop píše: ↑15 pro 2018, 15:28
Já bych to neviděl zas tak složitě.
Vyzkoušel bych přiložený
ideový návrh. Je to vlastně taky regulátor, řekněme regulátor s výrazně "integračním podílem". Je to vcelku jednoduché. Jen je potřeba si uvědomit, že dynamické parametry jsou extrémně závislé především na frekvenci vzorkování-porovnávání požadovaných a naměřených hodnot. Vliv má také dynamika motorů a vlastnosti snímače otáček.Proto jsem aplikoval knihovnu interval, která v případě správného použití v celém programu zajistí alespoň bezvadné časování. Dokáže nahradit funkci delay() a tím zefektivnit program o několik řádů.
Knihovna ve formě zip je dostupná
https://github.com/Pablo2048/Interval
Kód: Vybrat vše
#include <interval.h> // knihovna pro timing
#define PERIODA_REGULACE 10 // [ms]
int MotorPozado1; // pozadovana rychlost motoru 1
int MotorPozado2; // pozadovana rychlost motoru 2
int rpm_n1; // namerena - skutecna rychlost motoru 1
int rpm_n2; // namerena - skutecna rychlost motoru 2
int MotorControl1 = 0; // vystupni hodnota pro rizeni H mustku 1
int MotorControl2 = 0; // vystupni hodnota pro rizeni H mustku 2
Interval RegulaceMotor; // definice intervalu regulace motoru
void setup() {
// put your setup code here, to run once:
MotorPozado1 = 100; // nastaveni pozadovane rychlosti
MotorPozado2 = 100; // nastaveni pozadovane rychlosti
}
void loop() {
// put your main code here, to run repeatedly:
if (RegulaceMotor.expired()) {
RegulaceMotor.set(PERIODA_REGULACE);
if (rpm_n1 < MotorPozado1) { // pokud jsou otacky nizsi nez pozadovane, tak pridej
if (MotorControl1 < 255)MotorControl1++;
}
if (rpm_n1 > MotorPozado1) { // pokud jsou otacky vyssi nez pozadovane, tak uber
if (MotorControl1 > 0)MotorControl1--;
}
if (rpm_n2 < MotorPozado2) { // pokud jsou otacky nizsi nez pozadovane, tak pridej
if (MotorControl2 < 255)MotorControl2++;
}
if (rpm_n2 > MotorPozado2) { // pokud jsou otacky vyssi nez pozadovane, tak uber
if (MotorControl2 > 0)MotorControl2--;
}
}
}
Bohuzel me Vase regulace nefunguje. Netusite, kde by mohl byt problem.
#include <interval.h>
#define PERIODA_REGULACE 10
#include <AFMotor.h> //import your motor shield library
#define trigPin 4 // define the pins of your sensor
#define echoPin 5
#include <Servo.h>
#include <SoftwareSerial.h>
int encoder_pin_M1 = 2;
int encoder_pin_M2 = 3;
int MotorPozado1;
int MotorPozado2;
int MotorControl1 = 0;
int MotorControl2 = 0;
Interval RegulaceMotor;
unsigned int rpm_M1;
unsigned int rpm_M2;
volatile byte pulses_M1;
volatile byte pulses_M2;
unsigned long timeold_M1;
unsigned long timeold_M2;
unsigned int pulsesperturn = 20;
void counter_M1()
{
pulses_M1++;
}
void counter_M2()
{
pulses_M2++;
}
AF_DCMotor motor1(1,MOTOR12_8KHZ); // set up motors.
AF_DCMotor motor2(2, MOTOR12_8KHZ);
Servo myservo;
int pos = 0;
long duration;
int distance;
void setup()
{
myservo.attach(10);
Serial.begin(9600); // begin serial communitication
Serial.println("Motor test!");
pinMode(trigPin, OUTPUT);// set the trig pin to output (Send sound waves)
pinMode(echoPin, INPUT);// set the echo pin to input (recieve sound waves)
motor1.setSpeed(130); //set the speed of the motors, between 0-255
motor2.setSpeed (130);
pinMode(encoder_pin_M1, INPUT);
pinMode(encoder_pin_M2, INPUT);
attachInterrupt(0, counter_M1, FALLING);
attachInterrupt(1, counter_M2, FALLING);
pulses_M1 = 0;
pulses_M2 = 0;
rpm_M1 = 0;
rpm_M2 = 0;
timeold_M1 = 0;
timeold_M2 = 0;
MotorPozado1 = 300;
MotorPozado2 = 300;
Serial.begin(9600);
}
void loop()
{
if (millis() - timeold_M1 >= 1000)
{
detachInterrupt(0);
rpm_M1 = (60 * 1000 / pulsesperturn )/ (millis() - timeold_M1)* pulses_M1;
//Vypocet snimanych otacek
timeold_M1 = millis();
pulses_M1 = 0;
attachInterrupt(0, counter_M1, FALLING);
}
if (millis() - timeold_M2 >= 1000)
{
detachInterrupt(1);
rpm_M2 = (60 * 1000 / pulsesperturn )/ (millis() - timeold_M2)* pulses_M2;
timeold_M2 = millis();
pulses_M2 = 0;
attachInterrupt(1, counter_M2, FALLING);
}
for (pos = 60; pos <= 120; pos += 5)
{ // goes from 0 degrees to 180 degrees
// in steps of 1 degree
myservo.write(pos);
delay(10);
}
for (pos = 120; pos >= 50; pos -= 5)
{ // goes from 180 degrees to 0 degrees
myservo.write(pos); // tell servo to go to position in variable 'pos'
delay(10);
}
digitalWrite(trigPin, LOW);
delayMicroseconds(2); // delays are required for a succesful sensor operation.
digitalWrite(trigPin, HIGH);
delayMicroseconds(10); //this delay is required as well!
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;// convert the distance to centimeters.
if (distance < 25)
{
Serial.println ("Close Obstacle detected!" );
Serial.println ("Obstacle Details:");
Serial.print ("Distance From Robot is " );
Serial.print ( distance);
Serial.print ( " CM!");// print out the distance in centimeters.
Serial.println (" The obstacle is declared a threat due to close distance. ");
Serial.println (" Turning !");
motor1.run(FORWARD); // Turn as long as there's an obstacle ahead.
motor2.run (BACKWARD);
Serial.print("\t \t Measured Speed n1 [1/min] = " );
Serial.println((rpm_M1),DEC);
delay(10);
Serial.print("\t \t Measured Speed n2 [1/min] = " );
Serial.println((rpm_M2),DEC);
delay(10);
}
else
{
Serial.println ("No obstacle detected. going forward");
delay (15);
motor1.run(FORWARD); //if there's no obstacle ahead, Go Forward!
motor2.run(FORWARD);
Serial.print("\t \t Measured Speed n1 [1/min] = " );
Serial.println((rpm_M1),DEC);
delay(15);
Serial.print("\t \t Measured Speed n2 [1/min] = " );
Serial.println((rpm_M2),DEC);
delay(15);
}
if (RegulaceMotor.expired()) {
RegulaceMotor.set(PERIODA_REGULACE);
if (rpm_M1 < MotorPozado1) { // pokud jsou otacky nizsi nez pozadovane, tak pridej
if (MotorControl1 < 255)MotorControl1++;
}
if (rpm_M1 > MotorPozado1) { // pokud jsou otacky vyssi nez pozadovane, tak uber
if (MotorControl1 > 0)MotorControl1--;
}
if (rpm_M2 < MotorPozado2) { // pokud jsou otacky nizsi nez pozadovane, tak pridej
if (MotorControl2 < 255)MotorControl2++;
}
if (rpm_M2 > MotorPozado2) { // pokud jsou otacky vyssi nez pozadovane, tak uber
if (MotorControl2 > 0)MotorControl2--;
}
}
}