buongiorno aggiorno la situazione:
ho provato di nuovo a scrivere un codice (o comunque l'inizio) per questo robot e questa volta lo ho impostato mettendo il calcolo PID tra gli impulsi dei motori passo e calcolando quindi il tempo da trascorrere.
Per capirci meglio vi posto il codice:
/*********************************************************
Robot autobilancing
*********************************************************/
#include <Wire.h>
#include <PID_v1.h>
//valori giroscopio
#define MPU 0x68 // I2C address of the MPU-6050
double AcX, AcY, AcZ;
int Roll;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
double TIME_PID, TIME_PID_NOW, TIME_INT_PID = 20; //inserire valore millisecondi 1:45 tempo di caduta
double TIME_COUNTER_LOOP;
//Specify the links and initial tuning parameters
double Kp = 4,
Ki = 0,
Kd = 0;
//pin motori passo-passo
#define pinDirDX 2
#define pinStepDX 3
#define pinDirSX 4
#define pinStepSX 5
//pin motori passo-passo (velocità)
int VEL = 0;
#define STEP_MOTOR 5 //STEPPING PRIMA DI AGGIUSARE LA VELOCITà
//vallori velocita da impostare
#define vMax 40
#define vMin 450
#define M0d 7
#define M1d 6
#define M2d 8
#define M0s 10
#define M1s 9
#define M2s 11
//pin sensore ultrasuono
#define echoPort 14
#define triggerPort 15
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
void setup() {
Serial.begin(9600);
//motori passo-passo
pinMode( pinDirSX, OUTPUT );
pinMode( pinDirDX, OUTPUT );
pinMode( pinStepSX, OUTPUT );
pinMode( pinStepDX, OUTPUT );
//motori passo-passo (velocità)
pinMode( M0s, OUTPUT );
pinMode( M1s, OUTPUT );
pinMode( M2s, OUTPUT );
pinMode( M0d, OUTPUT );
pinMode( M1d, OUTPUT );
pinMode( M2d, OUTPUT );
//settaggio mortori
digitalWrite(M0s, LOW);
digitalWrite(M1s, LOW);
digitalWrite(M2s, LOW);
digitalWrite(M0d, LOW);
digitalWrite(M1d, LOW);
digitalWrite(M2d, LOW);
digitalWrite(M2s, HIGH);
digitalWrite(M2d, HIGH);
//sendore ultrasuoni
pinMode(triggerPort , OUTPUT);
pinMode(echoPort , INPUT);
//initialize the variables we're linked to
Setpoint = 0;
//turn the PID on
myPID.SetMode(AUTOMATIC);
//Input = abs(FunctionsMPU());
myPID.SetSampleTime(200);
//inizializazione giroscopio
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
delay(1000);
}
double FunctionsMPU() {
double DatoA, DatoB, Value;
Wire.beginTransmission(MPU);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // request a total of 14 registers
AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
DatoA = AcX;
DatoB = (AcY * AcY) + (AcZ * AcZ);
DatoB = sqrt(DatoB);
Value = atan2(DatoA, DatoB);
Value = Value * 180 / PI; //trasformare Rad in °
return (int)Value;
}
/*
void motorMove(){
delayMicroseconds(abs(velocita + TIME_PID));
TIME_PID=millis();
do{
digitalWrite( pinStepSX, HIGH );
digitalWrite( pinStepDX, HIGH );
delayMicroseconds(velocita);
digitalWrite( pinStepSX, LOW );
digitalWrite( pinStepDX, LOW );
delayMicroseconds(velocita);
TIME_PID_NOW=millis()-TIME_PID;
/
}while(TIME_PID_NOW<=TIME_INT_PID);
}*/
float PIDcalc() {
TIME_PID = micros();
Input=abs(FunctionsMPU());
//Input = 89 - abs(FunctionsMPU());
myPID.Compute(); //PID COUNTER
//VEL = map(constrain(Output , 0 , 255) ,0 ,255 ,vMax ,vMin);
VEL = vMin - (map(constrain(Output , 0 , 255) , 0 , 255 , vMax , vMin) - vMax);
if (Roll < 0) {
digitalWrite( pinDirSX, LOW );
digitalWrite( pinDirDX, HIGH );
} else if (Roll > 0) {
digitalWrite( pinDirSX, HIGH );
digitalWrite( pinDirDX, LOW );
}
return TIME_PID -= micros();
}
void motorSetMove() {
do {
Serial.println(Output);
digitalWrite( pinStepSX, HIGH );
digitalWrite( pinStepDX, HIGH );
delayMicroseconds(VEL - abs(PIDcalc()));
digitalWrite( pinStepSX, LOW );
digitalWrite( pinStepDX, LOW );
delayMicroseconds(VEL - abs(PIDcalc()));
} while (FunctionsMPU() != 0);
}
void loop() {
motorSetMove();
}
Il problemi principali sono due:
-
il primo è che non posso controllare il tempo tra un calcolo PID e il successivo
-
il secondo è che attualmente il mio risultato di quando calcolo il PID viene completamente senza senso e il 90 percento delle volte è zero
A questo punto non so se devo cambiare completamente l'impostazione del programma perché probabilmente sto sbagliando tutto, compreso il calcolo del PID, oppure continuare così .....
grazie per la risposta e arrivederci
(timer 0=8bit, timer 1=16bit, timer 2=8bit) é questo mi complica di molto le cose perché quelli che hanno fatto l'altro progetto lo sapevano e allora hanno preso la scheda leomardo e hanno utilizzato i due timer liberi da 16 bit (timer1 e timer3).
:P.