Buon pomeriggio a tutti!
Vi posto il codice che dovrebbe comandare un robottino:
#include <AFMotor.h>
#include <Servo.h>
#include <SoftwareSerial.h>
//const int LED_pinSX=13;
//const int LED_pinDX=10;
//const int LED_pinRear=7;
const int trigPin = 5;
const int echoPin =4;
const int speed = 20; //velocità in %
#define RX 10
#define TX 11
int data;
int data2;
Servo servo;
AF_DCMotor Motor_Left_Front(4,MOTOR34_1KHZ);
AF_DCMotor Motor_Right_Front(3,MOTOR34_1KHZ);
AF_DCMotor Motor_Left_Rear(1,MOTOR12_1KHZ);
AF_DCMotor Motor_Right_Rear(2,MOTOR12_1KHZ);
SoftwareSerial btSerial(RX,TX);
void setup() {
Serial.begin(9600);
//btSerial.begin(9600);
servo.attach(9);
//pinMode(LED_pinSX,OUTPUT);
//pinMode(LED_pinDX,OUTPUT);
//pinMode(LED_pinRear,OUTPUT);
servo.write(45);
Motor_Left_Front.setSpeed(255);
Motor_Right_Front.setSpeed(255);
Motor_Left_Rear.setSpeed(255);
Motor_Right_Rear.setSpeed(255);
}
void loop() {
Distance(trigPin,echoPin);
if(Serial.available()>0)
{
//data2=Serial.read();
//data2=btSerial.read();
//Serial.print(data2);
data=Serial.read();
//Serial.print(data2, HEX);
//Serial.print(" - ");
//Serial.println((char) data2);
switch(data)
{
//case '1'://LED ON
//digitalWrite(LED_pinSX,HIGH);
//digitalWrite(LED_pinDX,HIGH);
//digitalWrite(LED_pinRear,HIGH);
//break;
//case '2'://LED OFF
//digitalWrite(LED_pinSX,LOW);
//digitalWrite(LED_pinDX,LOW);
//digitalWrite(LED_pinRear,LOW);
//break;
case '3': //ServoMotor and Distance (HC-SR04)
ServoRotation();
//Distance();
break;
case 'F': //Forward
ServoRotation();
//Distance();
Serial.print("Vado avanti");
Motor_Left_Front.run(FORWARD);
Motor_Right_Front.run(FORWARD);
Motor_Left_Rear.run(FORWARD);
Motor_Right_Rear.run(FORWARD);
//digitalWrite(LED_pinSX,HIGH);
//digitalWrite(LED_pinDX,HIGH);
//digitalWrite(LED_pinRear,LOW);
break;
case 'B': //Backward
ServoRotation();
//Distance();
Motor_Left_Front.run(BACKWARD);
Motor_Right_Front.run(BACKWARD);
Motor_Left_Rear.run(BACKWARD);
Motor_Right_Rear.run(BACKWARD);
//digitalWrite(LED_pinSX,LOW);
//digitalWrite(LED_pinDX,LOW);
//digitalWrite(LED_pinRear,LOW);
break;
case 'S': //Stop
ServoRotation();
//Distance();
Motor_Left_Front.run(RELEASE);
Motor_Right_Front.run(RELEASE);
Motor_Left_Rear.run(RELEASE);
Motor_Right_Rear.run(RELEASE);
//digitalWrite(LED_pinSX,HIGH);
//digitalWrite(LED_pinDX,HIGH);
//digitalWrite(LED_pinRear,HIGH);
break;
case 'R': //Turn Right
ServoRotation();
//Distance();
Motor_Left_Front.run(BACKWARD);
Motor_Right_Front.run(BACKWARD);
Motor_Left_Rear.run(RELEASE);
Motor_Right_Rear.run(BACKWARD );
//digitalWrite(LED_pinSX,LOW);
//digitalWrite(LED_pinDX,HIGH);
break;
case 'L': //Turn Left
ServoRotation();
//Distance();
Motor_Left_Front.run(BACKWARD);
Motor_Right_Front.run(BACKWARD);
Motor_Left_Rear.run(BACKWARD);
Motor_Right_Rear.run(RELEASE );
//digitalWrite(LED_pinSX,HIGH);
//digitalWrite(LED_pinDX,LOW);
break;
}
}
}
void ServoRotation()
{
//senso orario
servo.write(0);
delay(300);
servo.write(45);
delay(300);
servo.write(90);
delay(300);
//anti-orario
servo.write(135);
delay(300);
servo.write(90);
delay(300);
servo.write(45);
delay(300);
}
void Distance(int TP, int EP)
{
long duration, inches, cm;
pinMode(TP, OUTPUT);
digitalWrite(TP, LOW);
delayMicroseconds(2);
digitalWrite(TP, HIGH);
delayMicroseconds(10);
digitalWrite(TP, LOW);
pinMode(EP, INPUT);
duration = pulseIn(EP, HIGH);
inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
Serial.print(inches);
Serial.print("in, ");
Serial.print(cm);
Serial.print("cm");
Serial.println();
delay(100);
}
long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
Il mio problema nasce dal fatto che vorrei avere una lettura dei dati da parte del sensore ultrasonico mentre si muove il servo (destra sinistra e centralmente).. la lettura me l'ha da anche ma se tipo gli mando il segnale 'F' (quindi di andare avanti) i motori non mi danno alcun segnale di vita... Sapete dirmi il perchè??
Grazie e buona giornata!