#define LeftWheelPin 6
#define RightWheelPin 5
#define ArmServoPin 4
#define SonarPin 0
#define runningLED 2
#define errorLED 7
short ArmAngle, dAngleMax,dAngleMin, dAngleMaxTemp, dAngleMinTemp;
short ArmAngleRange, ArmAngleTarget;
short LeftSpeed;
short RightSpeed;
short ArmTrim = 0;
int distance, dmax, dmin, dmaxtemp, dmintemp;
unsigned long lastPulseL;
unsigned long lastPulseR;
unsigned long lastPulseA;
int i, k;
unsigned short refreshTime = 15;
unsigned long timeLast, timeLastFast;
unsigned long timeLastSlow;
unsigned int timeStart;
// --------------------------------------------------- LED flashing
void flashLED(short pin, short times) {
for (i = 0; i<=times; i++) {
digitalWrite(pin, HIGH);
delay(100);
digitalWrite(pin, LOW);
delay(500);
}
}
// ---------------------------------------------------- Refreshes all servo pulses
void updateServos() {
if (millis() - lastPulseL >= refreshTime) {
digitalWrite(LeftWheelPin, HIGH);
delayMicroseconds((LeftSpeed+90)*11+500);
digitalWrite(LeftWheelPin, LOW);
lastPulseL = millis();
}
if (millis() - lastPulseR >= refreshTime) {
digitalWrite(RightWheelPin, HIGH);
delayMicroseconds((-RightSpeed+90)*11+500);
digitalWrite(RightWheelPin, LOW);
lastPulseR = millis();
}
if (millis() - lastPulseA >= refreshTime) {
digitalWrite(ArmServoPin, HIGH);
delayMicroseconds((ArmAngle+ArmTrim+90)*11+500);
digitalWrite(ArmServoPin, LOW);
lastPulseA = millis();
}
} // end updateServos
// ----------------------------------------------------- Rover move and turn function
void move(int speed, int turnSpeed) {
//LeftSpeed = speed - turnSpeed;
//RightSpeed = speed + turnSpeed;
if (LeftSpeed < 30 && LeftSpeed < (speed - turnSpeed)) {
LeftSpeed++;
//LeftSpeed = ((LeftSpeed*10)+1)/10
}
else if (LeftSpeed > -30 && LeftSpeed > (speed - turnSpeed)) {
LeftSpeed--;
//LeftSpeed = ((LeftSpeed*10)-1)/10;
}
if (RightSpeed < 30 && RightSpeed < (speed + turnSpeed)) {
RightSpeed++;
}
else if (RightSpeed > -30 && RightSpeed > (speed + turnSpeed)) {
RightSpeed--;
}
//if (speed == 0 && turnSpeed == 0) {
// LeftSpeed = 0;
// RightSpeed = 0;
//}
} // End move
// -------------------------------------------------- Update ArmAngle
void armturn() {
if (ArmAngle < 90 && ArmAngle < ArmAngleTarget) {
ArmAngle = ArmAngle ++;
}
else if (ArmAngle > -90 && ArmAngle > ArmAngleTarget) {
ArmAngle = ArmAngle --;
}
}
// ------------------------------------------------- sonar distance function
long ping() {
long raw;
raw = analogRead(SonarPin)+analogRead(SonarPin)+analogRead(SonarPin)+analogRead(SonarPin);
raw = raw/4;
return raw + raw*123/512;
}
// -------------------------------------------------- Main navigation function
void roving() {
int speed, turn;
randomSeed(millis());
if (dmax > 300 && abs(dAngleMax) < 10) {
ArmAngleRange = 15;
speed = 20;
turn = -dAngleMax;
}
else if (dmax > 60) {
ArmAngleRange = 60;
speed = 16;
turn = -dAngleMax/4;
}
else if (dmax > 30) {
ArmAngleRange = 70;
speed = dmax/5 - 6;
turn = -dAngleMax/24;
}
else if (dmax <= 30) {
ArmAngleRange = 80;
speed = 0;
turn = -dAngleMax/20;
digitalWrite(runningLED, HIGH);
}
if ((dmin <= 19)&&(abs(dAngleMin)<60)) {
ArmAngleRange = 80;
digitalWrite(errorLED, HIGH);
speed = -4;
if (dAngleMin !=0) {
turn = dAngleMin/20;
}
else {
if (random(0,1)) {
turn = -2;
}
else {
turn = 2;
}
}
}
/*
else {
move(0,0);
flashLED(errorLED,5);
}
*/
move(speed, turn);
}
void setup() {
pinMode(LeftWheelPin, OUTPUT);
pinMode(RightWheelPin, OUTPUT);
pinMode(ArmServoPin, OUTPUT);
pinMode(errorLED, OUTPUT);
pinMode(runningLED, OUTPUT);
pinMode(SonarPin, INPUT);
Serial.begin(9600);
LeftSpeed = 0;
RightSpeed = 0;
ArmAngleRange = 80;
ArmAngleTarget = 80;
distance = 50;
dmaxtemp = 20;
dmintemp = 1000;
flashLED(runningLED,5);
timeStart = millis();
move(0,0);
}
// -------------------------------------------------- Main Loop
void loop() {
digitalWrite(errorLED, LOW);
if (millis() - timeLast > 100) {
distance = ping();
if (distance < dmintemp) {
dmintemp = distance;
dAngleMinTemp = ArmAngle;
}
if (distance > dmaxtemp) {
dmaxtemp = distance;
dAngleMaxTemp = ArmAngle;
}
roving();
timeLast = millis();
updateServos();
}
if ((millis() - timeLastSlow > 500) && (abs(ArmAngle) >= abs(ArmAngleTarget))) {
if (ArmAngleTarget > 0) {
ArmAngleTarget = ArmAngleRange*-1;
}
else {
ArmAngleTarget = ArmAngleRange;
}
dAngleMax = dAngleMaxTemp;
dAngleMin = dAngleMinTemp;
dmax = dmaxtemp;
dmin = dmintemp;
dmaxtemp = 10;
dmintemp = 1000;
/*
Serial.print("Angles: ");
Serial.print(dAngleMax,DEC);
Serial.print(", ");
Serial.print(dAngleMin,DEC);
Serial.print(" - Distances: ");
Serial.print(dmax,DEC);
Serial.print(", ");
Serial.println(dmin,DEC);
*/
timeLastSlow = millis();
updateServos();
}
if (millis() - timeLastFast > 4) {
armturn();
timeLastFast = millis();
}
//updateServos();
//Serial.println(distance,DEC);
digitalWrite(runningLED, LOW);
updateServos();
}