The little one Code

05.02.2014
This is code Version v3. Here you find the the technical Equipment for this scetch: https://playground2014.wordpress.com/the-little-one-technic/

//Eyes_measurement
float distance;
float ld;
float rd;
float md;

float minDistance = 30; //define min distance to stopp the little one

byte robo_action;

int a1,b1,c1;

byte in1 = 3;
byte in2 = 4;
byte in3 = 5;
byte in4 = 6;
byte en12= 7;

byte Eyes_Trigger = 8; // I use a brown cable
byte Eyes_Echo = 9;
byte servo = 10;

byte Eyes_Error = 0;
byte auswahl;

int servoPos=1500;
int servoLeft = 1000;
int servoMiddle=1500;
int servoRight = 2000;

void setup() {
  Serial.begin(9600);
  pinMode(Eyes_Trigger, OUTPUT);
  pinMode(Eyes_Echo, INPUT);

  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(en12,OUTPUT);

 digitalWrite(in1, LOW);
 digitalWrite(in2, LOW);
 digitalWrite(in3, LOW);
 digitalWrite(in4, LOW);

 analogWrite(en12, 205);         // start a little bit slower

 //set timer1 interrupt at 10Hz
 TCCR1A = 0;
 TCCR1B = 0;
 TCNT1 = 0;
 OCR1A = 1562; // = 10Hz, must be less than 16 Hz!!!
 TCCR1B |= (1 << WGM12);
 TCCR1B |= (1 << CS12) | (1 << CS10);
 TIMSK1 |= (1 << OCIE1A);

 TCCR2A = 0;
 TCCR2B = 0;
 TCNT2 = 0;
 //OCR2A = 78 = 200,32 Hz = 4,992ms between two cycle
 //OCR2A = 79 = 197,74 Hz = 5,056ms between two cycle
 OCR2A = 78;
 TCCR2A |= (1 << WGM21);
 TCCR2B |= (1 << CS22) | (1 << CS20);
 TIMSK2 |= (1 << OCIE2A);  
 sei();            //allow Interrupts 
} 

ISR(TIMER1_COMPA_vect){   
  distance = Eyes_measurement();   
  robo_action=0;   
  if ( distance >= minDistance) { robo_action = 1; }
}

byte j = 0;
ISR(TIMER2_COMPA_vect){
  ++j;
  if (j == 1) {
    digitalWrite(servo,HIGH);
    delayMicroseconds(servoPos);
    digitalWrite(servo,LOW);
  }
  if (j == 40) {
    j = 0;
  }
}

float Eyes_measurement() {
  float distance;
  digitalWrite(Eyes_Trigger,LOW);
  delayMicroseconds(2);
  digitalWrite(Eyes_Trigger,HIGH);
  delayMicroseconds(10);
  digitalWrite(Eyes_Trigger,LOW);
  distance = pulseIn(Eyes_Echo,HIGH,40000);
  distance = distance / 58;
  return distance;
}

void chainsStop() {
  digitalWrite(in1, 0);
  digitalWrite(in2, 0);
  digitalWrite(in3, 0);
  digitalWrite(in4, 0);
}

int de1 = 400;
void servoMove() {
  servoPos=servoLeft;
  delay(de1);
  ld=distance;
  servoPos=servoRight;
  delay(de1);
  rd=distance;
  servoPos=servoMiddle;
  delay(de1);
  md=distance;
}

int obstacleWait = 380;

void obstacle() {
  servoMove();
  auswahl = 1;

  if (ld >= minDistance) { auswahl = 1; }
  if (rd >= minDistance) { auswahl = 9; }
  if (md >= minDistance) { auswahl = 100; }
  if (md <= minDistance) { auswahl = 5; }

 switch (auswahl) {
 case 1:
   //left rotation
   move_left();
   delay(obstacleWait);
   chainsStop();
   break;
 case 5:
   //back
   move_backward();
   delay(400);
   //right rotation
   move_right();
   delay(obstacleWait);
   chainsStop();
   break;
 case 9:
   //right rotation
   move_right();
   delay(obstacleWait);
   chainsStop();
   break;
 default:
   break;
 }
 delay(1000);
}

void move_forward() {
  PORTD = PORTD|B1001000;
}

void move_backward() {
  digitalWrite(in1, 0);
  digitalWrite(in2, 1);
  digitalWrite(in3, 1);
  digitalWrite(in4, 0);
}

void move_left() {
  digitalWrite(in1, 0);
  digitalWrite(in2, 1);
  digitalWrite(in3, 0);
  digitalWrite(in4, 1);
}

void move_right() {
  digitalWrite(in1, 1);
  digitalWrite(in2, 0);
  digitalWrite(in3, 1);
  digitalWrite(in4, 0);
}

void loop() {
  if (robo_action == 1) {
   Eyes_Error = 0;
   move_forward();
  };

 if (robo_action == 0) {
   ++Eyes_Error;
   chainsStop();
   delay(800);
   obstacle();
 };
}

29.01.2014

This is one of the first versions of the code.
With this code, the little one is moving. It’s stop if there an obstacle near by 25 cm. If an obstacle detected,  it moves backward and makes a small right rotation.
In this version, the servo is not connected.

//Eyes_measurement
float pulsdauer;
float distance;

float min_distance = 25.0;     //define min distance to stop the little one
byte robo_action = 1;
byte in1 = 3;      // left chain pin1
byte in2 = 4;      // left chain pin2
byte en12= 5;      // used with PWM for speed
byte in3 = 6;      // right chain pin1
byte in4 = 7;      // right chain pin2
byte servo = 8;    // temporary not in use
byte Eyes_Trigger = 9;   // my own reminder: I use a brown cable
byte Eyes_Echo = 10;

void setup() {
  pinMode(Eyes_Trigger, OUTPUT);
  pinMode(Eyes_Echo, INPUT);

  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(en12,OUTPUT);

  digitalWrite(in1, LOW);           // stop the left chain
  digitalWrite(in2, LOW);           // stop the left chain
  digitalWrite(in3, LOW);           // stop the right chain
  digitalWrite(in4, LOW);           // stop the right chain

  // define the Speed, start a little bit slower
  analogWrite(en12, 205);           

  cli();                           //stop Interrupts

  // define timer2
  TCCR2A = 0;
  TCCR2B = 0;
  TCNT2 = 0;
  OCR2A = 156;                    // 100 Hz = 156
  TCCR2A |= (1 << WGM21);
  TCCR2B |= (1 << CS22) | (1 << CS20);
  TIMSK2 |= (1 << OCIE2A); 
  sei();                          //allow interrupts
}

//timer2 function
ISR(TIMER2_COMPA_vect){
  Eyes_measurement();
  if (distance >= min_distance) {
   robo_action=1; } else {
   robo_action=0;
  }
}

void chain_stop() {
  digitalWrite(in1, 0);
  digitalWrite(in2, 0);
  digitalWrite(in3, 0);
  digitalWrite(in4, 0);
}

void Eyes_measurement() {
  digitalWrite(Eyes_Trigger,LOW);
  delayMicroseconds(2);
  digitalWrite(Eyes_Trigger,HIGH);
  delayMicroseconds(10);
  digitalWrite(Eyes_Trigger,LOW);
  pulsdauer = pulseIn(Eyes_Echo,HIGH);
  distance = pulsdauer / 58; // value in cm
}

void obstacle() {
  delay(1500);
  //both chains backward
  digitalWrite(in1, 0);
  digitalWrite(in2, 1);
  digitalWrite(in3, 1);
  digitalWrite(in4, 0);
  delay(1000);
  //right rotation
  digitalWrite(in1, 1);
  digitalWrite(in2, 0);
  digitalWrite(in3, 1);
  digitalWrite(in4, 0);
  delay(500);
  chain_stop();
  delay(1500);
}

void loop() {
 switch (robo_action) {
  case 0:
    TIMSK2 = 0; // interrup timer2 disable
    chain_stop();
    obstacle();
    TIMSK2 |= (1 << OCIE2A); // interrup timer2 enable
    break;
  case 1:
    //both chains forward
        /*
        digitalWrite(in1, 1);
        digitalWrite(in2, 0);
        digitalWrite(in3, 0);
        digitalWrite(in4, 1);
        */
    PORTD = B10001000; // the same like above, but faster *smile*
    break;
  }
}

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s