First test program for the AREXX AAR-04

/*
 Name project:   Small test
 Author:         -mat- filid brandy
 Company:        /
 Discription:     
 Robots drives a small pattern, blinks and follows a line
 Make sure that the AAR can __NOT__ move when uploading the program!
 After uploading the program, the AAR will wait 2 seconds before the program start. 
 
 >>> DO NOT FORGET TO REMOVE THE USB CABLE, after uploading the program.
 
 ATTENTION: THE ROBOT MOVES AROUND IN THIS EXAMPLE! PLEASE PROVIDE ABOUT
 1m x 1m OR MORE FREE SPACE FOR THE ROBOT! 
*/
#define  LeftBackward  10
#define  LeftForward  9
#define  RightForward  5
#define  RightBackward  6
#define  FrontLED  7
#define  LED2  8
#define  LED14  13
#define  SPEED 0x8F

int speedLeft,speedRight;
unsigned int lineData[2];
int ADOffset;

void setup()    
{
  //Setup: Initalization 
  pinMode(LeftForward, OUTPUT);
  pinMode(LeftBackward, OUTPUT);
  pinMode(RightForward, OUTPUT);
  pinMode(RightBackward, OUTPUT);
  
  // Front LED
  pinMode(FrontLED,OUTPUT);
  
  pinMode( LED2 ,OUTPUT); // LED2
  pinMode( LED14,OUTPUT); // LED14
    
  //Then set the start value of the signals to zero: 
  analogWrite(LeftBackward, 0);   
  analogWrite(LeftForward, 0);   
  analogWrite(RightForward, 0);   
  analogWrite(RightBackward, 0);
  
  //Startup delay:
  delay(2000);
}

void LineLeft(void) {
  speedLeft++;
  speedLeft = (speedLeft > 0xFE) ? 0xFF : speedLeft;
}

void LineRight(void) {
  speedRight++;
  speedRight = (speedRight > 0xFE) ? 0xFF : speedRight;
}


void FollowALine(int soLong) {
  int i;
  unsigned char j;
  int howLong = 0;
  
  digitalWrite(FrontLED, HIGH);
  stopMotor();

  
  for( j=0; j<0xFF; j++)
    LineData(lineData);
  LineData(lineData);
  
  ADOffset = lineData[0] - lineData[1];
  speedLeft = speedRight = SPEED;
  
  while ( howLong < soLong ) {
    howLong++;
    
    LineData(lineData);
    i = (lineData[0] - lineData[1]) - ADOffset;
    
    if ( i > 4 ) {
      LineLeft();
    } else if ( i < -4 ) {
      LineRight();
    } else {
      speedLeft = speedRight = SPEED;
    }
    
    analogWrite(LeftForward, speedLeft);
    analogWrite(RightForward, speedRight);
  }
  
  digitalWrite(FrontLED, LOW);
  stopMotor();
}

void LineData(unsigned int *linedata) {
  linedata[0] = analogRead(A6); // left linesensor
  delay(10);
  linedata[1] = analogRead(A7); // right linesensor
  delay(10);
}


void rechts(int soLong, int soFast) {
   digitalWrite(RightForward,LOW);
   analogWrite(RightBackward,soFast);
   digitalWrite(LeftForward,LOW);
   digitalWrite(LeftBackward,LOW);
   delay(soLong);
}

void links(int soLong,int soFast) {
   digitalWrite(LeftForward,LOW);
   analogWrite(LeftBackward,soFast);
   digitalWrite(RightForward,LOW);
   digitalWrite(RightBackward,LOW);
   delay(soLong);
}

void turnOnSpot(int soLong,int soFast) {
   digitalWrite(LeftForward,LOW);
   analogWrite(LeftBackward,soFast);
   digitalWrite(RightBackward,LOW);
   analogWrite(RightForward,soFast);
   delay(soLong);
}

void turnOnSpotCCW(int soLong,int soFast) {
   digitalWrite(LeftBackward,LOW);
   analogWrite(LeftForward,soFast);
   digitalWrite(RightForward,LOW);
   analogWrite(RightBackward,soFast);
   delay(soLong);
}

void rueckwaerts(int soLong,int soFast) {
   digitalWrite(LeftForward,LOW);
   analogWrite(LeftBackward,soFast);
   digitalWrite(RightForward,LOW);
   analogWrite(RightBackward,soFast);
   delay(soLong); //drive for 5000ms
}

void vorwaerts(int soLong, int soFast) {
   digitalWrite(LeftBackward,LOW);
   analogWrite(LeftForward,soFast);
   digitalWrite(RightBackward,LOW);
   analogWrite(RightForward,soFast);
   delay(soLong);
}

void stopMotor() {
  digitalWrite(LeftForward,LOW); digitalWrite(LeftBackward,LOW); //stop motors
  digitalWrite(RightForward,LOW); digitalWrite(RightBackward,LOW); //stop motors   
}

void blinkLEDs(int soOften) {
  for (int i=0; i<soOften; i++ ) {
    digitalWrite(LED2,  LOW);
    digitalWrite(LED14, HIGH);
    delay(100);
    digitalWrite(LED2,  HIGH);
    digitalWrite(LED14, LOW);
    delay(100);
  }
  digitalWrite(LED2,  LOW);
  digitalWrite(LED14, LOW);
}

void loop() {
  for (int i=0;i<4;i++) {
    vorwaerts(2000,200);
    links(1000,200);
  }
  turnOnSpotCCW(2000,200);
  stopMotor();
  blinkLEDs(1);

  for (int i=0;i<4;i++) {
    rueckwaerts(2000,200);
    rechts(1000,200);
  }
  turnOnSpot(2000,200);
  stopMotor();

  blinkLEDs(10);
  FollowALine(1000);
  blinkLEDs(10);

}