Salve a tutti!
Ho un problema con il software del mio robot bilanciante.
Il progetto prevede che il robot sia in grado di tenersi in equilibrio e ricevere dei comandi via bluetooth.
Tuttavia ho un problema con l'uso del Bluetooth. Uso il servizio UART della ap nrf toolbox per comunicare. Il bluetooth per ora va a scrivere in base ai comandi su alcune variabili, ma non effettua nessuna vera modifica nel comportamento del robot. Tuttavia assisto ad un fenomeno particolare: Il robot si bilancia come atteso fino a che non connetto l'app del cellulare. Quando la connessione viene stabilita il robot non è piu in grado di tenersi in equilibrio. Basta interrompere la comunicazione per riportare il robot al normale funzionamento. Non ho idea del perché ciò accada. Premetto che sono nuovo del forum e che questo è il mio primo progetto con genuino 101.
Il codice è il seguente:
double Setpoint = 0;
double inputAngle = 0;
double outputPwm1;
int outMax = 255;
int outMin = -255;
float lastInput = 0;
double ITerm =0;
// Costanti del PID
double kp = 45; // costante Proporzionale
double ki = 0; // costante Integrale
double kd = 0; // costante Derivativa
// definizione dei pin per il comando dei motori
int motorSspeed = 6;
int motorDspeed = 5;
int motorDA = 11;
int motorDB = 12;
int motorSA = 9;
int motorSB = 8;
// fine definizione dei pin motori
#include <CurieBle.h> // include la libreria per la comunicazione BLE
BLEPeripheral blePeripheral; // BLE Peripheral Device (the board you're programming)
BLEService UART("6E400001-B5A3-F393-E0A9-E50E24DCCA9E"); // Imposta il codice identificativo del servizio BLE UART Service
// BLE UART Characteristic
BLEUnsignedCharCharacteristic TX("6E400002-B5A3-F393-E0A9-E50E24DCCA9E", BLEWrite);
BLEUnsignedCharCharacteristic RX("6E400003-B5A3-F393-E0A9-E50E24DCCA9E", BLENotify | BLERead);
const int ledPin = 13; // pin di comando del LED on-board di arduino
char sendingChar = 0x41; // variabile per l'invio di caratteri tramite bluetooth
char txChar = 0;
#include "CurieImu.h" // include la libreria per l'utilizzo di accelerometro e giroscopio
#include <Math.h> // include la libreria per l'utilizzo di funzioni matematiche
double angolo = 0; // variabile per il salvataggio delle misure di angolo
int16_t ax, ay, az;
int16_t gx, gy, gz;
int pwww;
int minimo = 0;
int velocita = 0;
double dt = 0; // differenziale di tempo per il calcolo di integrazione nel filtro complementare
unsigned long t = 0;
unsigned long prevT = 0;
// variabili per comandi movimento:
int muovi = 0;
//Subroutine che calcola il valore dell'uscita del PID
double compute(double input)
{
double error = Setpoint - input;
ITerm = ITerm + error * ki; // calcolo dell'integrale
if (ITerm > outMax) ITerm = outMax; // se il termine integrale è maggiore o minore
if (ITerm < outMin) ITerm = outMin; // del limite massimo o minimo, viene riportato nel range
double dInput = (input - lastInput);
double output = kp * error + ITerm + kd * dInput;
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
// ricorda l'input precedente
lastInput = input;
return output;
}
void setup() {
Serial.begin(9600);
//inizializzazione e calibrazione IMU
//
CurieImu.initialize();
CurieImu.autoCalibrateGyroOffset();
CurieImu.autoCalibrateXAccelOffset(0);
CurieImu.autoCalibrateYAccelOffset(0);
CurieImu.autoCalibrateZAccelOffset(1);
CurieImu.setGyroOffsetEnabled(true);
CurieImu.setAccelOffsetEnabled(true);
//
// fine calibrazione IMU
// impostazione come pin di output dei pin di comando motori e LED
pinMode(motorDspeed, OUTPUT);
pinMode(motorSspeed, OUTPUT);
pinMode(motorDA, OUTPUT);
pinMode(motorDB, OUTPUT);
pinMode(motorSA, OUTPUT);
pinMode(motorSB, OUTPUT);
pinMode(ledPin, OUTPUT);
// fine impostazione OUTPUT
// imposta nome Bluetooth della scheda and service UUID:
blePeripheral.setLocalName("BB");
blePeripheral.setAdvertisedServiceUuid(UART.uuid());
// aggiunge i servizi e le caratteristiche:
blePeripheral.addAttribute(UART);
blePeripheral.addAttribute(TX);
blePeripheral.addAttribute(RX);
// imposta il valore iniziale delle caratteristiche:
TX.setValue(0);
RX.setValue(128);
// avvia la comunizazione BLE:
blePeripheral.begin();
}
void loop() {
digitalWrite(ledPin, HIGH);
t = micros();
dt = t - prevT;
dt = dt / 1000000;
prevT = t;
CurieImu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
angolo = atan( ay / sqrt(pow(ax, 2) + pow(az, 2))); //calcolo dell'angolo
angolo = (360 * angolo) / 6.28 ;
inputAngle = angolo;
if (inputAngle > 2.5 || inputAngle < -2.5 ) kp = 28;
else kp = 48;
outputPwm1 = compute(inputAngle);
sendingChar = angolo;
RX.setValue(sendingChar + '0');
BLECentral central = blePeripheral.central();
if (central) {
if (central.connected()) {
if (TX.written())
{
txChar = TX.value();
switch (txChar)
{
case '0' :
muovi = 0;
break;
case '1' :
muovi = 0;
break;
case '2' :
muovi = 1;
break;
case '3' :
muovi = 0;
break;
case '4' :
muovi = 2;
break;
case '5' :
muovi = 0;
break;
case '6' :
muovi = 3;
break;
case '7' :
break;
case '8' :
break;
case '9' :
muovi = 0;
break;
}
}
}
}
if (outputPwm1 < 0)
{
digitalWrite(motorSA, HIGH);
digitalWrite(motorSB, LOW);
digitalWrite(motorDA, HIGH);
digitalWrite(motorDB, LOW);
velocita = -outputPwm1;
velocita = floor(velocita);
pwww = velocita + minimo;
if (pwww > 255) pwww=255;
analogWrite(motorSspeed, pwww);
analogWrite(motorDspeed, pwww);
}
else if (outputPwm1 > 0) // se l'uscita del PID è positiva
{
digitalWrite(motorSA, LOW);
digitalWrite(motorSB, HIGH);
digitalWrite(motorDA, LOW);
digitalWrite(motorDB, HIGH);
velocita = outputPwm1;
velocita = floor(velocita);
if (avvio > 0)
{
velocita = velocita + 100;
avvio--;
}
pwww = velocita + minimo;
if (pwww > 255) pwww=255;
analogWrite(motorSspeed, pwww);
analogWrite(motorDspeed, pwww);
}
}
Grazie mille in anticipo