Buongiorno,
sto svolgendo una tesi in cui ho preso un automodello radiocomandato, ho sostituito il computer presente all'interno con un arudino nano e ho aggiunto varie altre componenti (driver di potenza,mocoder,modulo bluetooth RN-42). Il passo successivo è stato quello di integrare al progetto due sensori sonar HC-SR04 per determinare e mantenere una certa distanza tra due automodelli. Problema: una volta collegati i due sensori sonar all'alimentazione di arduino il servo inizia a gira da una parte all'altra senza sosta e i sensori non determinano la distanza. Ho collegato inizialmente fili del servo e quelli dei sonar alla stessa alimentazione (5V) e alla stessa GND, poi ho provato ad usare un arduino esterno come alimentatore per i due sensori sonar ma senza risultato positivo. Vi allego il codice perchè a questo punto non capisco se sia un problema hardware o software.
#include <SoftwareSerial.h>
#include "constants1.h"
#include "Tweakly.h"
//Variabili sistema di controllo servo
float servomotorvalue, degree, mydegree, servoerror; //K = guadagno controllore proporzionale servo
float K=8;
float offset;
//Filtro a media mobile servo
float servofilter[servofsize];
int i = 0;
float servoavg = 0.0; //average
volatile unsigned long count;
unsigned long oldcount = 0;
unsigned long t1, dt;
float velocity; //velocita' encoder
unsigned long iter = 1;
void isr() {
count++;
}
//Filtro a media mobile mocoder
int const motorfsize = 5;
float motorfilter[motorfsize];
int i1 = 0;
//Variabili controllore PI
float myv, motorerror, motorvalue;
float Kp = 10, Ki = 1, integer = 0.0;
float m = pow(10, -3);
//Filtro a media mobile distanza
int const dsize = 10;
float distancefilter[dsize];
int i2 = 0;
//Variabili sistema di controllo distanza
float myd = 10;
float distance_error1, distance_error2;
float Kd=10;
void setup() {
Serial.begin(9600);
delay(1000);
Serial.println("Started");
pinMode(pinRx, INPUT);
pinMode(pinTx, OUTPUT);
pinMode(servopwm, OUTPUT);
pinMode(servodir, OUTPUT);
pinMode(potref, INPUT);
pinMode(motorpwm, OUTPUT);
pinMode(motordir, OUTPUT);
pinMode(interruptPin, INPUT);
bluetooth.begin(115200);
delay(100);
bluetooth.print("$$$"); //
delay(100);
bluetooth.println("U,9600,N"); //
delay(100);
bluetooth.end();
delay(100);
bluetooth.begin(9600);
delay(100);
bluetooth.println("---<cr>"); //
attachInterrupt(digitalPinToInterrupt(interruptPin), isr, RISING);
sonarAttach(pin_echo1, pin_trigger1, getSonarValue1, SONAR_CENTIMETERS);
sonarAttach(pin_echo2, pin_trigger2, getSonarValue2, SONAR_CENTIMETERS);
}
void loop() {
TweaklyRun();
ReadBT();
computeVelocity();
ControlloServo();
ControlloMotore();
ControlloDistanze();
}
void ControlloServo() {
degree = ((ServoAverage() - MinAnalogRef) * (MaxSteeringAngle - MinSteeringAngle) / (MaxAnalogRef - MinAnalogRef) + MinSteeringAngle);
degree += Offset();
servoerror = mydegree - degree;
servomotorvalue = abs((K * servoerror));
if (servomotorvalue < minPwm) {
analogWrite(servopwm, 0);
} else {
if (degree <= mydegree) {
digitalWrite(servodir, HIGH); //gira a destra
analogWrite(servopwm, servomotorvalue);
} else {
digitalWrite(servodir, LOW); //gira a sinistra
analogWrite(servopwm, servomotorvalue);
}
}
}
float ServoAverage() { //media mobile
int s = analogRead(potref);
servofilter[i] = s;
if (i < (servofsize - 1)) i++;
else i = 0;
servoavg = 0.0;
for (int j = 0; j < servofsize; j++) {
servoavg += (float)servofilter[j];
}
return servoavg = servoavg / (float)(servofsize);
}
float Offset() {
return offset = motorvalue * max_offset / maxPwm;
}
void ReadBT() {
while (bluetooth.available() > 0) {
String command = bluetooth.readString();
if (command != "") {
char op = command.c_str()[0];
command.replace(String(op), "");
float value = command.toFloat();
Serial.println("op = " + String(op) + " value = " + String(value));
switch (op) {
case 's':
mydegree = value;
break;
case 'r':
digitalWrite(motordir, LOW);
myv = value;
integer = 0;
break;
case 'f':
digitalWrite(motordir, HIGH);
myv = value;
integer = 0;
break;
default: break;
}
}
}
}
void computeVelocity() {
dt = millis() - t1;
if (dt >= interval) {
noInterrupts();
float ve = (float)(count - oldcount) / (float)(interval);
velocity = ve * gr * wheelcircumference; //cm/s
oldcount = count;
t1 = millis();
interrupts();
}
}
void getSonarValue1(unsigned long centimeters) {
//Print sonar value
distance1 = centimeters;
}
void getSonarValue2(unsigned long centimeters) {
//Print sonar value
noInterrupts();
distance2 = centimeters;
interrupts();
}
float MotorAverage() { //Media mobile velocita'
motorfilter[i1] = velocity;
if (i1 < (motorfsize - 1)) {
i1++;
} else {
i1 = 0;
}
float motoravg = 0.0;
for (int j1 = 0; j1 < motorfsize; j1++) {
motoravg += motorfilter[j1];
}
motoravg = motoravg / (float)(motorfsize);
return motoravg;
}
void ControlloMotore() { //Controllo Proporzionale Integrale
float motAvg = MotorAverage();
motorerror = myv - motAvg;
motorvalue = Kp * motorerror + integer;
integer = Ki * (integer + (motorerror * (interval * m)));
motorvalue = constrain(motorvalue, 0, maxPwm);
analogWrite(motorpwm, (uint8_t)motorvalue);
}
void ControlloDistanze() {
Serial.println(distance1);
Serial.println(distance2);
}
):