Salve a tutti.
Vorrei sapere se qualcuno mi potrebbe dare una mano sul codice in questione: dovrei implementare un codice di Wall following di un robot omniwheel, attraverso 4 ultrasonar (hy srf05) e avendo scritto il codice, non capisco dove sbaglio, si tratta di una board Arduino Duemilanove modificata, progettata appositamente per il robot, dotata di un microcontrollore ATmega328 inoltre ho un serio problema a utilizzare le librerie degli interrupt, dato che sia i sensori che i motori condividono lo stesso pin. Se qualcuno mi potesse dare una mano, mi farebbe un gran favore
questo e' il codice:
#define _NAMIKI_MOTOR
#include <MotorWheel.h>
#include <Omni3WD.h>
#include <Omni4WD.h>
#include <PID_Beta6.h>
#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>
#include <NewPing.h>
/* Convenzione per ruote, velocità (notare i versi che indicheranno la direzione di advance per le ruote)
__
/| \
wheel1 / \ wheel4
v0 / _\| v3
power switch
__
|\ /
wheel2 \ / wheel3
v1 \ |/_ v2
*/
irqISR(irq1,isr1); // Interrupt function on the basis of the pulse, work for wheel1
MotorWheel wheel1(3,2,4,5,&irq1); //This will create a MotorWheel object called Wheel1
//Motor PWM:Pin5, DIR:Pin4, Encoder A:Pin12, B:Pin13
irqISR(irq2,isr2);
MotorWheel wheel2(11,12,14,15,&irq2);
irqISR(irq3,isr3);
MotorWheel wheel3(9,8,16,17,&irq3);
irqISR(irq4,isr4);
MotorWheel wheel4(10,7,18,19,&irq4);
Omni4WD Omni(&wheel1,&wheel2,&wheel3,&wheel4);
#define SONAR_NUM 4 // numeri di sonar usati
#define MAX_DISTANCE 800 // la distanza massima tra l'oggetto e il sensore
#define PING_INTERVAL 33 // il tempo tra i ping dei sensori espresso in millesecondi
//definisco i pin dei sensori
#define trigPin1 12
#define echoPin1 11
#define trigPin2 8
#define echoPin2 9
#define trigPin3 0
#define echoPin3 1
#define trigPin4 10
#define echoPin4 9
NewPing sonar[SONAR_NUM] = { // Define a Newping array to measure the distance
NewPing(trigPin1, echoPin1, MAX_DISTANCE),
NewPing(trigPin2, echoPin2, MAX_DISTANCE),
NewPing(trigPin3, echoPin3, MAX_DISTANCE),
NewPing(trigPin4, echoPin4, MAX_DISTANCE)
};
void setup() {
Serial.begin(9600);
delay(4000);
TCCR1B=TCCR1B&0xf8|0x01; // Pin9,Pin10 PWM 31250Hz
TCCR2B=TCCR2B&0xf8|0x01; // Pin3,Pin11 PWM 31250Hz
Omni.PIDEnable(0.31,0.01,0,10); // Enable PID
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
digitalWrite(trigPin1, LOW);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
digitalWrite(trigPin2, LOW);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
digitalWrite(trigPin3, LOW);
pinMode(trigPin4, OUTPUT);
pinMode(echoPin4, INPUT);
digitalWrite(trigPin4, LOW);
}
int vicinoalmuro = 1800; //Initialze and define the values for distance between wall and robot
int lontanodalmuro = 2500;
int vicinomurodavanti = 1000;
int rangeSinistra = 0;
int rangeDavanti = 0;
int rangeDestra = 0;
int rangeIndietro = 0;
void loop() {
{
Main:
rangeDavanti = readRangeDavanti();
Serial.print(rangeDavanti);
Serial.print(" Davanti ");
Serial.println();
rangeSinistra = readRangeSinistra();
Serial.print(rangeSinistra);
Serial.print(" Sinistra ");
Serial.println();
rangeDestra = readRangeDestra();
Serial.print(rangeDestra);
Serial.print(" Destra ");
Serial.println();
rangeIndietro = readRangeIndietro();
Serial.print(rangeIndietro);
Serial.print(" Indietro ");
Serial.println();
}
if (rangeDavanti < vicinomurodavanti && rangeSinistra < vicinoalmuro ) //Condizione per vedere se il robot e vicino al muro sia da davanti che a sinistra, quindi trovo uno spigolo
{
delay(500);
indietro();
delay(500);
Serial.print(" Indietro ");
destra();
Serial.print(" Destra ");
Serial.println();
delay(800);
avanti();
delay(1700);
goto Main;
}
if (rangeDavanti < vicinomurodavanti && rangeDestra < vicinoalmuro ) //Condizione per vedere se il robot e vicino al muro sia da davanti che a destra, quindi trovo uno spigolo
{
delay(500);
indietro();
delay(500);
Serial.print(" Indietro ");
sinistra();
Serial.print(" Sinistra ");
Serial.println();
delay(800);
avanti();
delay(1700);
goto Main;
}
if ( rangeDavanti < vicinomurodavanti && rangeDestra > vicinoalmuro || rangeDavanti < vicinomurodavanti && rangeSinistra > vicinoalmuro )
{ //condizione in cui sono davanti al muro
delay(500);
indietro();
delay(500);
Serial.print(" Indietro ");
sinistra();
Serial.print(" Sinistra ");
Serial.println();
delay(800);
avanti();
delay(1700);
goto Main;
}
if (rangeIndietro < vicinomurodavanti) //Condition to check whether front sensor is close to robot
{
delay(500);
avanti();
delay(500);
Serial.print(" avanti ");
}
if(rangeSinistra > vicinoalmuro && rangeSinistra < lontanodalmuro){ //condizione per controllare se la distranza vicino al muro e mantenuta correttamente
avanti();
Serial.print(" avanti");
goto Main;
}
if(rangeDestra > vicinoalmuro && rangeDestra < lontanodalmuro) //condizione per controllare se la distranza vicino al muro e mantenuta correttamente
{
avanti();
Serial.print(" avanti ");
goto Main;
}
if (rangeSinistra < vicinoalmuro ) //sono vicino al muro di sinistra
{
delay(100);
destra();
delay(500);
Serial.print(" destra");
avanti();
Serial.print(" avanti ");
Serial.println();
goto Main;
}
if (rangeDestra < vicinoalmuro ) //condition to check side wall is close to robot
{
delay(100);
sinistra();
delay(500);
Serial.print(" Sinistra ");
avanti();
Serial.print(" avanti ");
Serial.println();
goto Main;
}
if (rangeSinistra > lontanodalmuro && rangeDestra < lontanodalmuro ) //ccondizione per il quale il robot e lontano dal muro
{
delay(100);
sinistra();
Serial.print(" sinistra ");
delay(500);
avanti();
Serial.print(" avanti");
Serial.println();
goto Main;
}
if (rangeDestra > lontanodalmuro && rangeSinistra < lontanodalmuro) //condition to check robot is far from side wall
{
delay(100);
destra();
Serial.print(" destra");
delay(500);
avanti();
Serial.print(" avanti");
Serial.println();
goto Main;
}
}
void avanti() // function to drive the robot forward
{
Omni.setCarAdvance(200);
Omni.delayMS(17);
}
void indietro() // function to drive the robot backward
{
Omni.setCarBackoff(200);
Omni.delayMS(17);
}
void sinistra() // function to turn the robot left
{
Omni.setCarLeft(200);
Omni.delayMS(17);
}
void destra() //function to turn the robot right
{
Omni.setCarRight(200);
Omni.delayMS(17);
}
int readRangeDavanti() //function to read the front sensor value
{
delay(50);
unsigned rangeDavanti = sonar[0].ping();
sonar[0].timer_stop();
return rangeDavanti;
}
int readRangeSinistra() // function to read the left sensor value
{
delay(50);
unsigned rangeSinistra = sonar[1].ping();
sonar[1].timer_stop();
return rangeSinistra;
}
int readRangeDestra() //function to read the front sensor value
{
delay(50);
unsigned rangeDestra = sonar[2].ping();
sonar[2].timer_stop();
return rangeDestra ;
}
int readRangeIndietro() //function to read the front sensor value
{
delay(50);
unsigned rangeIndietro = sonar[3].ping();
sonar[3].timer_stop();
return rangeIndietro;
}
