Hi everyone
please can you help me modify this code to make a 7 segment led's count as the whisker move around and indicate number of times a robot meets obstacles.
#include <avr/io.h> //include avr io definitions
#define piezo 10
#include <Servo.h>
void forward(unsigned int time); //full speed forward for a duration of time (ms)
void turnLeft(unsigned int time); // Left turn for a duration of time (ms)
void turnRight(unsigned int time); // Right turn for a duration of time (ms)
void backward(unsigned int time); // Full-speed Backward for a duration of time (ms)
void disableServos(); // Halt servo signals
Servo servoLeft; // Declare left and right servos
Servo servoRight;
void setup() // Built-in initialization block
{
Serial.begin(9600);
tone(piezo, 3000, 1000); // Play tone for 1 second
delay(1000); // Delay to finish tone
DDRB &= 0b11111100; //set pin 8 and 9 as inputs
servoLeft.attach(11); // Attach left signal to pin 11
servoRight.attach(12); // Attach right signal to pin 12
}
void loop() // Main loop auto-repeats
{
char message[80];
char leftWhisker;
char rightWhisker;
char TOUCHED;
leftWhisker = PINB & 0x01; //pin 8 (PINB0) is connected to the left whisker
rightWhisker = (PINB & 0x02)>>1; //pin 9 (PINB1) is connected to the right whisker
if( ( leftWhisker == TOUCHED ) && ( rightWhisker == TOUCHED ) ) // both whiskers are touched, servo move backward for 1s then pivot turn left.
{
backward(1000); // go backward for 1 sec
pivotLeft(1000); // pivot left for 1 sec
}
else if ( leftWhisker == TOUCHED) // left whisker is touched
{
backward(1000); // go backward for 1 sec
turnRight(500); // turn right for 0.5 sec
}
else if ( rightWhisker == TOUCHED ) // right whisker is touched
{
backward(1000); // go backward for 1 sec
turnLeft(500); // turn left for 0.5 sec
}
else // no whisker is touched
{
forward(20); // go forward fot 20ms
}
}
void pivotLeft(int time) // pivot left turn function
{
servoLeft.writeMicroseconds(1500); // Left wheel counerclockwise
servoRight.writeMicroseconds(1300); // Right wheel clockwise
delay(time);
}
void forward(unsigned int time) // Full-speed Forward function
{
servoLeft.writeMicroseconds(1700); // Left wheel counerclockwise
servoRight.writeMicroseconds(1300); // Right wheel clockwise
delay(time); // Maneuver for time ms
}
void turnLeft(unsigned int time) // Left turn function
{
servoLeft.writeMicroseconds(1300); // Left wheel clockwise
servoRight.writeMicroseconds(1300); // Right wheel clockwise
delay(time); // Maneuver for time ms
}
void turnRight(unsigned int time) // Right turn function
{
servoLeft.writeMicroseconds(1700); // Left wheel counterclockwise
servoRight.writeMicroseconds(1700); // Right wheel counterclockwise
delay(time); // Maneuver for time ms
}
void backward(unsigned int time) // Full-speed Backward function
{
servoLeft.writeMicroseconds(1300); // Left wheel clockwise
servoRight.writeMicroseconds(1700); // Right wheel counterclockwise
delay(time); // Maneuver for time ms
}
void disableServos() // Halt servo signals
{
servoLeft.detach(); // Stop sending servo signals
servoRight.detach();
}