Sto scrivendo il codice di un self balancing robot controllato da un MPU6050 e due driver DM860I.
Passo dopo passo sto vedendo se funziona facendomi stampare sul seriale i valori delle variabili.
IL PROBLEMA è L'ABILITAZIONE DEI MOTORI ALL'ACCENSIONE.
Io voglio che i motori del robot si attivino all'accensione sono quando lo metto in piedi.
La variabile booleana START che dovrebbe diventare TRUE all'accensione solo se la stessa è FALSE e l'inclinazione del robot è compresa tra -2° e 2°, non diventa mai TRUE, anche se l'ho inizializzata come FALSE e anche se vedo sul seriale che il valore assoluto l'angolo Alpha è < 2°.
Come è possibile?
Dove sbaglio?
#include <Wire.h>
#define MPU 0x68
#define EN 9
#define POWER 10
#define DIR 11
#define STEP 0.45
#define TEq 1000000
#define TEmer 1500000
boolean START=0;
boolean EMERGENCY=false;
double AY=0.,AZ=0.;
double Alpha=0.;
double ApprSTEPS=0.;
double BETWSTEPS=0.;
double OVER=0.;
void setup(){
Serial.begin(9600);
init_MPU();
init_DM860I();
}
void loop()
{
FunctionsMPU(); // Acquisisco accelerazioni AY, AZ
Alpha = CalcAlpha(AY,AZ); //Calcolo angolo dalle 2 componenti di accelerazione
if (Alpha>=0) //Comunico la direzione giusta ai drivers in relazione allo squilibrio
digitalWrite(DIR,false);
else
digitalWrite(DIR,true);
delayMicroseconds(5); //tempo logico di comprensione del DM860I
Alpha=abs(Alpha); //Alpha come intensità di squilibrio
//############### Approssimazione angolo in uno divisibile per STEP #################
ApprSTEPS=Alpha/STEP;
if(ApprSTEPS-(long)ApprSTEPS<=0.5)
ApprSTEPS =(long)ApprSTEPS;
else
ApprSTEPS=((long)ApprSTEPS)+1;
Alpha=ApprSTEPS*STEP;
//############### ABILITAZIONE MOTORI ALL'ACCENSIONE ######################
if((START==false)&&(Alpha<=2.00)){
digitalWrite(EN,true);
START==true;
EMERGENCY==0;
}
else
digitalWrite(EN,false);
delayMicroseconds(5); //tempo logico di comprensione del DM860I
//############### RUN ##############################
if(Alpha!=0&&START==1){
BETWSTEPS=(TEq/(Alpha/STEP)); //Calcolo tempo delay tra due step
//in modo che la velocità angolare sia pari
//al numero degli step che IO si discosta dall'equilibrio/1s
//EX. Alpha = 18.90° --> N step per equibibrio = 18.90/0.45 = 42 STEPS
//Ritardo tra due steps = 1000000 Microsecondi / 42 STEPS = 23809,5 Microsecondi
//Velocità = 18.90°/s
//In 1 secondo IO deve compiere 18.90° e tornare in equilibrio
stepMotore();
delayMicroseconds(BETWSTEPS);
}
Serial.print("\nAlpha: "); Serial.print(Alpha);
Serial.print("\ndelayMicroseconds: "); Serial.print(BETWSTEPS);
if(START==1) { Serial.print("\nSTART: "); Serial.print("1"); } //Stampa START
else { Serial.print("\nSTART: "); Serial.print("0"); }
delay(1000);
}
//#################### FUCTIONS PER MPU ##################
void init_MPU(){
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);
}
//Funzione per l'acquisizione degli assi Y,Z del MPU6050
void FunctionsMPU(){
Wire.beginTransmission(MPU);
Wire.write(0x3D); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU,4,true); // request a total of 14 registers
AY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AZ=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
}
//Funzione per il calcolo degli angolo
double CalcAlpha(double A, double B){
double DatoA, DatoB, Value;
DatoA = A;
DatoB = B;
Value = atan2(DatoB, DatoA);
Value = Value * (180/3.14159);
return (double)Value;
}
//#################### FUCTIONS PER STEPPER ##################
void init_DM860I(){
digitalWrite(EN,false);
digitalWrite(DIR,false);
}
void stepMotore(){
digitalWrite(POWER, HIGH);
delayMicroseconds(5);
digitalWrite(POWER, LOW);
delayMicroseconds(5);
}