Good day, im having a problem with a program (my com port is picking wrong values)...
here's the code
#include <math.h>
#include <Servo.h>
//Constants
#define BAUD0 9600 //USB FTDI baud rate
#define ECHOPIN0 5 //Echo pin 0
#define ECHOPIN1 6 //Echo pin 1
#define MINPULSE 600 //Minimum servo pulse
#define MAXPULSE 2400 //Maximum servo pulse
#define TRIGPIN0 7 //Trig pin 0
#define TRIGPIN1 8 //Trig pin 1
Servo servo0; //Creating servo object servo0
Servo servo1; //Creating servo object servo1
//Variables
bool sdelay = false;
int delaytime = 50;
int cm0, precm0=1;
int cm1, precm1=1;
int val0, val1, maxdeg;
long duration0;
long duration1;
//PING DDC
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
//DEBUGG
void debugg()
{
Serial.print(val0);
Serial.print(" ");
Serial.print(val1);
Serial.println();
}
//SETUP
void setup()
{
Serial.begin(BAUD0);
pinMode(TRIGPIN0, OUTPUT);
pinMode(ECHOPIN0, INPUT);
pinMode(TRIGPIN1, OUTPUT);
pinMode(ECHOPIN1, INPUT);
servo0.attach(9, MINPULSE, MAXPULSE);
servo1.attach(10, MINPULSE, MAXPULSE);
}
//MAIN
void loop()
{
//Ping sensor 0 reading
digitalWrite(TRIGPIN0, LOW);
delay(2);
digitalWrite(TRIGPIN0, HIGH);
delay(5);
digitalWrite(TRIGPIN0, LOW);
duration0 = pulseIn(ECHOPIN0, HIGH);
cm0 = microsecondsToCentimeters(duration0);
cm0 = map(cm0, 5, 20, 5, 20);
//Ping sensor 1 reading
digitalWrite(TRIGPIN1, LOW);
delay(2);
digitalWrite(TRIGPIN1, HIGH);
delay(5);
digitalWrite(TRIGPIN1, LOW);
duration1 = pulseIn(ECHOPIN1, HIGH);
cm1 = microsecondsToCentimeters(duration1);
cm1 = map(cm1, 5, 20, 5, 20);
//Mapping the readings to the corresponding servo angles
maxdeg = map(cm0, 20, 5, 0, 90);
val0 = map(cm1, 5, 20, maxdeg, 0);
val1 = map(cm1, 5, 20, 0, maxdeg);
switch(cm0)
{
case 20:
if(20-precm0>=5) //Hand removed quickly from the ultrasonic sensor (moved from the line of sensor reading)
{
servo0.write(val0);
sdelay=true;
}
break;
default: //Default action (hand moved on the "line of sensor reading")
servo0.write(val0);
sdelay=true;
precm0=cm0;
break;
}
switch(cm1)
{
case 20:
if(20-precm1>=5)
{
servo1.write(val1);
sdelay=true;
}
break;
default:
servo1.write(val1);
sdelay=true;
precm1=cm1;
break;
}
//Writing the values to the servos, applying the delays (if needed), and monitoring the results via UART
servo0.write(val0);
servo1.write(val1);
if(sdelay==true) delay(delaytime);
sdelay=false;
debugg();
}
I think the way of mine thinking is pretty clear, but if any doubts i can further explain...
thanks