Code:
<code>
// Compact, a small Arduino (Duemilanove) robot
// 2 Pololu micro motors 100:1 and small wheels,
// one HXT900 micro servo, one Sharp IR sensor,
// 2 AAA battery holders, the Robot Builder’s Shield
//
// Arduino pinout:
//
// Shield Funct Arduino ATmega168 Arduino Funct Shield
// ±----/----+
// Reset 1| PC6 PC5 |28 D19 A5 SCL
// Rx D0 2| PD0 PC4 |27 D18 A4 SDA
// Tx D1 3| PD1 PC3 |26 D17 A3
// Int0 D2 4| PD2 PC2 |25 D16 A2
// Int1 D3 5| PD3 PC1 |24 D15 A1
// M1B D4 6| PD4 PC0 |23 D14 A0 IR sensor
// 7| VCC GND |22
// 8| GND AREF |21
// Xtal 9| PB6 AVCC |20
// Xtal 10| PB7 PB5 |19 D13 SCK LED
// M1A OC0B D5 11| PD5 PB4 |18 D12 MISO Pan servo
// M2A OC0A D6 12| PD6 PB3 |17 D11 OC2A MOSI
// M2B D7 13| PD7 PB2 |16 D10 OC1B
// D8 14| PB0 PB1 |15 D 9 OC1A
// ±----------+
//
#include <Servo.h>
//Inputs/outputs
#define Motor_1_PWM 5 // digital pin 5 // Right Motor
#define Motor_1_Dir 4 // digital pin 4
#define Motor_2_PWM 6 // digital pin 6 // Left Motor
#define Motor_2_Dir 7 // digital pin 7
#define IR_Pin 14 // digital pin 14 (analog pin 0)
#define PanPin 12
#define LedPin 13
#define SR 1 //Sharp Short Range sensor
#define MR 2 //Sharp Medium Range sensor
#define LR 3 //Sharp Long Range sensor
#define center 90
//Variables
byte dir=0;
byte speed1=250;
byte speed2=255;
int turn90=110;
int turn45=55;
int straight=500;
int stopTime=200;
int IRdistance=0;
int treshold=20; //20cm min distance
Servo Pan;
//-----------------------------------------------------------------------------
void setup() {
// set motor pins as output and LOW so the motors are breaked
pinMode(Motor_1_PWM, OUTPUT);
pinMode(Motor_1_Dir, OUTPUT);
pinMode(Motor_2_PWM, OUTPUT);
pinMode(Motor_2_Dir, OUTPUT);
Stop();
Pan.attach(PanPin);
Pan.write(center); //90
StepDelay();
pinMode(LedPin, OUTPUT);
digitalWrite(LedPin, LOW);
Serial.begin (19200);
Serial.println(“start”);
Forward();
}
void loop(){
Drive();
//square(); //use this function to adjust the timings for turns
//and to make sure the robot is driving in straight lines
}
void square(){
Forward();
delay(straight);
Stop();
delay(stopTime);
Right();
delay(turn90);
Stop();
delay(stopTime);
Forward();
delay(straight);
Stop();
delay(stopTime);
Right();
delay(turn90);
Stop();
delay(stopTime);
Forward();
delay(straight);
Stop();
delay(stopTime);
Right();
delay(turn90);
Stop();
delay(stopTime);
Forward();
delay(straight);
Stop();
delay(stopTime);
Right();
delay(turn90);
Stop();
delay(stopTime);
}
//--------------------------
void Drive(){
IRdistance=Read_Sharp_Sensor(MR, IR_Pin);
Serial.print("IRdistance ");
Serial.println(IRdistance);
if (IRdistance<10){
Stop();
StepDelay();
TurnAround();
}
if (IRdistance<treshold){
Stop();
StepDelay();
Avoid();
Forward();
}
delay(50);
}
void TurnAround(){
Reverse();
Pan.write(center);
StepDelay();
Stop();
Left();
delay(turn90);
delay(turn90);
Stop();
StepDelay();
Forward();
}
void Avoid(){
int prev=0;
dir=2;
for (byte i=0; i<5; i++){
Pan.write(i45);
StepDelay();
StepDelay();
IRdistance=Read_Sharp_Sensor(MR, IR_Pin);
if (IRdistance>prev){
dir=i;
prev=IRdistance;
}
}
Pan.write(center);
StepDelay();
switch (dir){
case 0:
Right();
delay(turn90);
Stop();
StepDelay();
break;
case 1:
Right();
delay(turn90); //turn45
Stop();
StepDelay();
break;
case 2:
Forward();
break;
case 3:
Left();
delay(turn90); //turn45
Stop();
StepDelay();
break;
case 4:
Left();
delay(turn90);
Stop();
StepDelay();
break;
}
delay(500);
}
// Read Sensors
int Read_Sharp_Sensor(byte model, byte pin) {
int value = 0;
value = analogRead(pin);
switch (model) {
case SR: //short range, aka GP2D120 (4-30cm)
return (2914/(value+5))-1;
break;
case MR: //medium range, aka GP2D12 (10-80cm)
return 51384.4pow(value,-.9988); //I had to multiply by 5, different sensor
break;
case LR: //long range, aka GP2Y0A02YK (20-150cm)
return 11441pow(value,-.9792);
break;
}
}
void StepDelay() {
for (byte t=0; t<10; t++){
delay(20);
}
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void Forward(){
digitalWrite(Motor_1_Dir, LOW); // forward
digitalWrite(Motor_2_Dir, LOW); // forward
analogWrite(Motor_1_PWM, speed1); //
analogWrite(Motor_2_PWM, speed2); //
return;
}
void Reverse(){
digitalWrite(Motor_1_Dir, HIGH); // reverse
digitalWrite(Motor_2_Dir, HIGH); // reverse
analogWrite(Motor_1_PWM, 255-speed1); //
analogWrite(Motor_2_PWM, 255-speed2); //
return;
}
void Right(){
digitalWrite(Motor_1_Dir, HIGH); // reverse
digitalWrite(Motor_2_Dir, LOW); // forward
analogWrite(Motor_1_PWM, 255-speed1); //
analogWrite(Motor_2_PWM, speed2); //
return;
}
void Left(){
digitalWrite(Motor_1_Dir, LOW); // forward
digitalWrite(Motor_2_Dir, HIGH); // reverse
analogWrite(Motor_1_PWM, speed1); //
analogWrite(Motor_2_PWM, 255-speed2); //
return;
}
void Stop()
{
digitalWrite(Motor_1_PWM, LOW);
digitalWrite(Motor_1_Dir, LOW);
digitalWrite(Motor_2_PWM, LOW);
digitalWrite(Motor_2_Dir, LOW);
return;
} </code>
