Begginner robot programming course arduino code. (JYU, December/2012) Photos: http://www.flickr.com/photos/93259094@N05/sets/72157634761276095/ Video: http://youtu.be/jIrg4B3pWpg

gistfile1.ino

/*

========== GPIO WIRING ==========
===== INPUT =====
0 Battery input voltage (halved) (TODO: stop robot if voltage < 6.8V)
1 Bump (unused)
2
3
4
5
===== OUTPUT =====
4-7 Motor Shield
9  Steering servo
10 Head servo
11 Sonar ECHO
12 Sonar TRIGGER
13 Alert LED (debugging)

*/

#include <Servo.h> 
#include <NewPing.h>

//Sonar setup
#define TRIGGER_PIN  12  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     11  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 500 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

unsigned int uS1;
//unsigned int uS2;



//Motor Shield setup
//Motor 1
int mRightPower = 5; //Suunta
int mRightDirection = 4; //Nopeus
//Motor 2
int mLeftPower = 6;                     
int mLeftDirection = 7;                       

int joypinY = 1;
int motorVal;

//Steering servo
int joypinX = 0;
int steerVal;
Servo sSteering;

//Head servo
Servo sHead;

int getDistanceCm() //TODO douple ping for average
{
  //delay(50);                      // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  uS1 = sonar.ping(); // Send ping, get ping time in microseconds (uS).
//  delay(100);
//  uS2 = sonar.ping();
  Serial.print("Ping: ");
  Serial.print(uS1 / US_ROUNDTRIP_CM); // Convert ping time to distance in cm and print result (0 = outside set distance range)
//  Serial.print(" and ");
 // Serial.print(uS2 / US_ROUNDTRIP_CM);
  Serial.println("cm");
  return(uS1 / US_ROUNDTRIP_CM);
}

//------------------Testing
void tattiohjaus() //For testing the joystick-control
{
   steerVal = analogRead(joypinX);
   steerVal = map(steerVal, 0, 1023, 40, 139);
   sSteering.write(steerVal);
   Serial.print("steerVal");
   Serial.println(steerVal);
   
   motorVal = analogRead(joypinY);
   motorVal = map(motorVal, 0, 1023, -255, 255); 
                      
                      //Jos mennään eteenpäin niin...
    if (motorVal > 0) { digitalWrite(mRightDirection,HIGH); digitalWrite(mLeftDirection,HIGH);   }
    else              { digitalWrite(mRightDirection,LOW);digitalWrite(mLeftDirection,LOW); }

    analogWrite(mRightPower, abs(motorVal));
    analogWrite(mLeftPower, abs(motorVal)); 
}



void alert(String alertmessage)
{
    digitalWrite(13,HIGH); 
    delay(50);
    digitalWrite(13,LOW); 
    Serial.println(alertmessage);
}


//TODO: return a collision information
void drive(int cm, int angle){
  sHead.write(80);
  Serial.print("Driving ");
  Serial.print(cm);
  Serial.print(" at angle of ");
  Serial.print(angle); 
  
  sSteering.write((angle + 90) - 5); // Small correction to go straight, TODO: better correction
  delay(200);       
                      //If going forward..
  if (cm > 0) { digitalWrite(mRightDirection,LOW); digitalWrite(mLeftDirection,LOW);   }
  else        { digitalWrite(mRightDirection,HIGH);digitalWrite(mLeftDirection,HIGH); }

  motorVal = 255;
  analogWrite(mRightPower, abs(motorVal));
  analogWrite(mLeftPower, abs(motorVal)); 
  delay(abs(cm * 8)); //Magical multiplier value for converting length to time
  analogWrite(mRightPower, 0);
  analogWrite(mLeftPower, 0); 
  
}

void turn(int degrees){
  //Decide turning direction
  //If positive degrees -> turn right
  if (degrees > 0) { digitalWrite(mRightDirection,HIGH); 
                     digitalWrite(mLeftDirection,LOW);   
                     sSteering.write(1);}
  //If negative degrees -> turn left
              else { digitalWrite(mRightDirection,LOW);
                     digitalWrite(mLeftDirection,HIGH); 
                     sSteering.write(178);}

  delay(400); 
  
  analogWrite(mRightPower, 250);
  analogWrite(mLeftPower, 250);
  delay(abs(degrees)*3);
  analogWrite(mRightPower, 0);
  analogWrite(mLeftPower, 0);
  sSteering.write(85);

}

void sweepSector() //TODO return array from here and continue with it with other functions
{
//Scan surroundings
  alert("Starting scan");
  sHead.write(0);
  delay(400); //TODO: testing with big value

  int readings[5]; 

  for (int a = 0, i = 0; a < 180; a = a+35) {
  sHead.write(a);
  delay(300);
  Serial.print(i);
  readings[i++] = getDistanceCm();
  delay(50);
  }
  sHead.write(80);


//Choose best way to go

//Find max value
  int max = 0; //Maximum value
  int maxi = 0;//Index of maximum value
  //i=0;
  for (int i1=0; i1<6; i1++){
    
    if(readings[i1]>max){
      max = readings[i1];
      maxi = i1;
    }
    if(readings[i1]==0){  //Zero is always propably the longest distance
      max = 999; //TODO: ugly
      maxi = i1;}
    }
  
  // if (max = 0) {turn(180); break;} //if sonar didn't get a proper reading

  if ( (readings[2] < 20 && readings[2] != 0) || (readings[3] < 20 && readings[2] != 0) ){

    alert("turning 180");
    drive(-40,0); 
    delay(1000);
    //TODO: choose turning direction according to readings got from sides of the robot
    turn(200); 
    return;
  }
   

  switch (maxi){
    case 0:
    alert("case0");
    turn(-130);
    drive(60,0);
    break;
  
    case 1:
    alert("case1");
    turn(-90);
    drive(60,0);
    break;
  
    case 2:
    alert("case2");
    //turn(-45);
    drive(60,0);
    break;
  
    case 3:
    alert("case3");
    //turn(-45);
    drive(60,0);
    break;
  
    case 4:
    alert("case4");
    turn(80);
    drive(60,0);
    break;
  
    case 5:
    alert("case5");
    turn(130);
    drive(60,0);
    break;
  
    default:
    alert("case def");
    break;}
}


// ==================== MAIN PROGRAM ====================
void setup()
{
    Serial.begin(9600); // Open serial monitor to see debug results.
    //pinMode(mRightDirection, OUTPUT);  
    //pinMode(mLeftDirection, OUTPUT);
    sSteering.attach(9);
    sHead.attach(10);

    // drive(50,0);
    // delay(42000);
    // turn(-180);
    // delay(1000);
    // drive(50,0);
    // turn(-110);

    
    //choosePath(readings);

    alert("Starting");
}

void loop()
  {
  //tattiohjaus();
  sweepSector();
  delay(1000);
  }