Hi guys, I am a noob of coding and I am trying to script:
There is a robot with two sensors ir in front ultrasonic sfr02 in back. If something pass back or in front of the robot it has to accelerate from standby to the maxspeed, then do some random steers and return in standby.
here it is the code:
#include <MegaServo.h>
#include <Wire.h>
//SFRO2 ultrasonic
#define sensorAddress 0x70
#define readCentimeters 0x51
#define readMicroseconds 0x52
#define resultRegister 0x02
//
int stdledPin = 13; // choose the pin for the LED
int irFrontPin = 0; // choose the input pin (for a pushbutton)
int irFrontval = 0; // variable for reading the ir pin data
int speedLeft = 0; // speed we want on left servo
int speedRight = 0; // speed we want on right servo
boolean freeFront = true; // forward path
boolean freeBack = true; // backward path
boolean standBy = true; // standby mode
boolean moveBack = false; // backward mode
boolean moveForward = false; // forward mode
unsigned long time; // time
long interval = 1000; // ?
long previousMillis = 0; // ?
MegaServo left;
MegaServo right;
void setup() {
Wire.begin(); // start I2C bus
pinMode(stdledPin, OUTPUT); // declare LED as output
pinMode(irFrontPin, INPUT); // declare pushbutton as input
left.attach(10);
right.attach(11);
Serial.begin(9600);
}
void targetFront() { // there is someone in front of RADIYOSP
standBy = false; // we are no longer in standBy
freeFront = false; // The front path isn't free
moveBack = true; // we are going backward
int i = 1; // additional val
for (; i < 90; i++) {
digitalWrite(stdledPin, HIGH); //light up stdled
left.write(89 - i); //from standby position substract every cycle 0.1 (the servo start to go backward)
right.write(90 + i); //from standby position add every cycle 0.1 (the servo start to go backward)
delay(10);
}
if (i = 89) {
left.write(random(0,180)); //from standby position substract every cycle 0.1 (the servo start to go backward)
right.write(random(0, 180)); //from standby position add every cycle 0.1 (the servo start to go backward)
delay(random(100, 1000));
standFor();
}
}
void targetBack() { // there is someone in the back of RADIYOSP
standBy = false;
freeBack = false;
moveBack = false; // we are no longer in standby we are going backward
moveForward = true;
int i = 0.0001;
for (; i < 90; i++) {
digitalWrite(stdledPin, HIGH); //don't light up stdled
left.write(90 - i); //from standby position substract every cycle 0.1 (the servo start to go backward)
right.write(0 + i); //from standby position add every cycle 0.1 (the servo start to go backward)
}
if (i = 89) {
left.write(random(0, 45)); //from standby position substract every cycle 0.1 (the servo start to go backward)
right.write(random(45,135)); //from standby position add every cycle 0.1 (the servo start to go backward)
delay(random(100, 1000));
standFor();
}
}
/*void takeatrip() {
standBy = false;
moveForward = true;
float i = 1; // variable to add for accelerate servos
left.write(180); //from standby position substract every cycle 0.1 (the servo start to go backward)
right.write(0); //from standby position add every cycle 0.1 (the servo start to go backward)*/
void standFor() { // standby mode (no movements)
left.write(90); // stop servoleft
right.write(90); // stop servoright
standBy = true; // we are in standby
digitalWrite(stdledPin, LOW); // light up stdled
freeFront = true; // front is free we can navigate forward
moveBack = false; // we are in standby
moveForward = false;
freeBack = true;
}
void payattention() {
irFrontval = analogRead(irFrontPin); // read ir input value
sendCommand(sensorAddress, readCentimeters); // send the command to read the result in centimeters:
delay(70); // wait at least 70 milliseconds for a result:
setRegister(sensorAddress, resultRegister); // set the register that you want to reas the result from:
int sensorReading = readData(sensorAddress, 2); // read the result:
delay(70);
if (irFrontval > 100) { // if irfront input is > 100
standBy = false;
freeFront = false;
//delay(500 + random(100, 3000));
}
if (sensorReading < 60) { // if ultra (SFR 02) get something
standBy = false;
freeBack = false;
}
}
void loop(){
standFor();
payattention();
if (standBy = false) {
if(freeFront = false) {
targetFront();
}
if (freeBack = false) {
targetBack();
}
}
if (standBy = true) {
standFor();
}
// take a look around
/*if (time - previousMillis > interval) {
previousMillis = millis();
takeatrip();
delay(500);*/
}
//I2C COM
void sendCommand (int address, int command) { // start I2C transmission:
Wire.beginTransmission(address);
Wire.send(0x00);
Wire.send(command);
Wire.endTransmission();
}
//SET INPUT ADDRESS
void setRegister(int address, int thisRegister) {
Wire.beginTransmission(address); // start I2C transmission:
Wire.send(thisRegister); // send address to read from:
Wire.endTransmission();
}
//READ DATA
int readData(int address, int numBytes) { //readData() returns a result from the SRF sensor
int result = 0; // the result is two bytes long
Wire.requestFrom(address, numBytes); // send I2C request for data:
while (Wire.available() < 2 ) { // wait for two bytes to return:
// wait for result
}
result = Wire.receive() * 256; // read the two bytes, and combine them into one int:
result = result + Wire.receive();
return result; // return the result:
}
I try to use For statement but i don't think it works, it don't seems accelerate.
