Hi all! I have a little project I want to finish, and it consists basicaly of 3 small DC motors being controlled by 3 Ultrasonic sensors (SRF02). I am using a Diecimilla board and I am using PWM to control the motor speed, and the code is simply 4 cases meaning 4 power profiles for the motor speed depending on distance (it is just fery basic code with a bunch of ifs). I am using Serial monitor to check what distance the sensors are measuring (between 12-300 cm)
The problem I am having right now is that sometimes the whole thing sometimes just hangs, motors keep spinning etc. Also the code seems badly optimized enough that it doesn't register the distance correctly etc.
Connections are the standard I2C connection for the sensors (using analog 5, 4 for SDA and SCL, 5v and GND) and the motors are connected to a power phase using 4N26 optoacoplators on PWM pins 9, 10 and 11.
Here is the code, any optimization, help, suggestion is more than welcome! (The final code will not have the section of Serial Monitor, that is just temporary to see what it is doing)
#include "Wire.h"
#include "SRF02.h"
SRF02 srf02[3]=
{
SRF02(0x70, SRF02_CENTIMETERS),
SRF02(0x71, SRF02_CENTIMETERS),
SRF02(0x72, SRF02_CENTIMETERS)
};
unsigned long nextPrint = 0;
int distance = 0; // Distance measured by the SRF02 1
int distance2 = 0; // Distance measured by the SRF02 2
int distance3 = 0; // Distance measured by the SRF02 2
int ledPin = 9; // PWM out for digital pin 9
int led2Pin = 10; // PWM out for digital pin 10
int led3Pin = 11; // PWM out for digital pin 11
int fadeValue = 0; //PWM value for first motor on pin 9
int fade2Value = 0; //PWM value for first motor on pin 10
int fade3Value = 0; //PWM value for first motor on pin 11
void setup() {
Serial.begin(9600);
Wire.begin();
SRF02::setInterval(500);
}
void loop() {
SRF02::update();
fadeValue=0;
fade2Value=0;
fade3Value=0;
analogWrite(ledPin, fadeValue);
analogWrite(led2Pin, fade2Value);
analogWrite(led3Pin, fade3Value);
distance=srf02[0].read();
distance2=srf02[1].read();
distance3=srf02[2].read();
if(distance==0)
{distance=400;}
if(distance >=130)
{
if (distance >=250)
{fadeValue=0;
analogWrite(ledPin, fadeValue);
}
if(distance <=249)
{
fadeValue=100;
analogWrite(ledPin, fadeValue);
}
}
if(distance <=129)
{if(distance >=71)
{
fadeValue=170;
analogWrite(ledPin, fadeValue);
}
if(distance <=70);
{
fadeValue=250;
analogWrite(ledPin, fadeValue);
}
}
//Second sensor conditions begin here
if(distance2==0)
{distance2=400;}
if(distance2 >=130)
{
if (distance2 >=250)
{fade2Value=0;
analogWrite(led2Pin, fade2Value);
}
if(distance2 <=249)
{
fade2Value=100;
analogWrite(led2Pin, fade2Value);
}
}
if(distance2 <=129)
{if(distance2 >=71)
{
fade2Value=170;
analogWrite(led2Pin, fade2Value);
}
if(distance2 <=70);
{
fade2Value=250;
analogWrite(led2Pin, fade2Value);
}
}
//third sensor here
if(distance3==0)
{distance3=400;}
if(distance3 >=130)
{
if (distance3 >=250)
{fade3Value=0;
analogWrite(led3Pin, fade3Value);
}
if(distance3 <=249)
{
fade3Value=100;
analogWrite(led3Pin, fade3Value);
}
}
if(distance3 <=129)
{if(distance3 >=71)
{
fade3Value=170;
analogWrite(led3Pin, fade3Value);
}
if(distance3 <=70);
{
fade3Value=250;
analogWrite(led3Pin, fade3Value);
}
}
//This section reads and displays the distance picked up by the sensor using the Serial Monitor
SRF02::update();
if (millis() > nextPrint)
{
Serial.print(srf02[0].read());
Serial.print(",");
Serial.print(srf02[1].read());
Serial.print(",");
Serial.print(srf02[2].read());
Serial.println();
nextPrint = millis () + 1000;
}
}