Arduino.ru

Отличная тема! Вы нам простыню, а мы Вам:

Вот видео работы машинки

http://www.youtube.com/watch?v=7l6YK-FRkJc

Вот ПО:

/* Программа управления машинкой с ультразвуковым датчиком рассояния на сервомоторе и двумя сервомоторами на колесах. */
#include<avr/io.h>

// Просто мигаем светодиодом
const int led_pin =  5;      // PB5 номер выхода, подключенного к светодиоду

// Variables will change:
volatile long startMks = 0;
volatile long stopMks = 0;      // храним время последнего переключения светодиода
volatile boolean dist_m = false;
byte scanSpeed = 1000;           // интервал между включение/выключением светодиода (1 десятая секунды)
// скорость машинки
signed short int w_speed = 15;


#include <Servo.h>
Servo LeftServoMotor;  // create servo object to control a left servo
Servo RightServoMotor;  // create servo object to control a left servo
Servo USMMotor;  // create servo object to control a left servo

// Pin 3 - сигнальная ножка мотора правого колеса
#define rightMotorPin  3
// Pin 5 - сигнальная ножка мотора левого колеса
#define leftMotorPin  5
// Pin 6 - сигнальная ножка мотора который вращает ультразвуковой датчик
#define usdMotorPin  6

// блок констант для измерения расстояния
#define trig_Pin 1 //PINB1 ножка под триггер УЗ датчика
#define echo_Pin  2 //PINB2 ножка под эхо УЗ датчика

//массив сектора расстояний
unsigned int sector_distance[6] = {1,1,1,1,1};
// расстояние в текущем направлении 
volatile unsigned int distance_sm=0; 



// нулевая точка правой и левой сервы
int right_Servo_Zero_Point = 68;
int left_Servo_Zero_Point = 72;

// как припаяны провода на моторчиках (вращаются в одну сторону или в разные)
boolean motorSinc = false;


// the setup routine runs once when you press reset:
void setup() {    
  
  DDRB = B0100010; // выход на триггер (B1) и LED-диод (B5) эхо (В2) на ввод
  DDRD = B01101000; //выходы на сервы (D3) и (D5) и на серву глаза D6 

  //настраиваем прерывание под установку PB в ноль (триггер)

  /*The External Interrupt 0 is activated by the external pin INT0 if the SREG I-flag and the corresponding
   interrupt mask are set.*/
  EICRA = B10; // External Interrupt Control Register A . The falling edge of INT0 generates an interrupt request.
  /*
  When the PCIE0 bit is set (one) and the I-bit in the Status Register (SREG) is set (one), pin
   change interrupt 0 is enabled.
   */
  //  EIMSK = B1; // EIMSK – External Interrupt Mask Register. Bit 0 – INT0: External Interrupt Request 0 Enable
  PCICR = B1; //PCICR – Pin Change Interrupt Control Register. Any change on any enabled PCINT7..0 pin will cause an interrupt. Глобально разрешает/запрещает прерывания INT2/1/0
  PCMSK0 = B100; //PCMSK0 Bit 7..0 – PCINT7..0: Pin Change Enable Mask 7..0 Включаем прерывание на эхе (по спаду сигнала - это определено выше)

  pinMode(rightMotorPin, OUTPUT); 
  pinMode(leftMotorPin, OUTPUT); 
  pinMode(usdMotorPin, OUTPUT); 

  //
  LeftServoMotor.attach(leftMotorPin);  // attaches the servo
  RightServoMotor.attach(rightMotorPin);  // attaches the servo
  USMMotor.attach(usdMotorPin);  // attaches the servo

  // initialize serial:
  Serial.begin(9600);
  // останавливаем колеса
  stop_mashine();
  // выставляем глаз перед собой
  USMMotor.write(100);
  delay(3000);
  distance_sm =0;
}

// the loop routine runs over and over again forever:
void loop() {
   
  terrain_scan(); //заполняет массив препятствий на пути
  byte alfa = path_select(); // возвращает угол поворота 0, -30, -60, 60, 30
  Serial.print(alfa);
  Serial. print("\n");
  rotate(alfa);
  move_forvard();
  while(look_arraund()){  
  delay(100);
  };
  stop_mashine();
  
  }


void stop_mashine(){ 
  int lms = left_Servo_Zero_Point;
  int rms = right_Servo_Zero_Point;
  LeftServoMotor.write(lms);
  RightServoMotor.write(rms);
}



boolean look_arraund(){
  
  ultrasonic();
  delay(100);
  if (distance_sm > 15){
    Serial.print(distance_sm);
    Serial.print("\n");
    return true;
  }
  else return false;
}

 
byte path_select(){
  
  byte k=0;
  unsigned int j=0;
  
  for (byte i=0;i<5;i++){
   if (j<=sector_distance[i]) {
     k = i;
     j=sector_distance[i];
   }
  }
 return k;
}

void terrain_scan(){
  
 for (byte i = 0; i < 5; i++){
  USMMotor.write(100-60+i*30);
  delay(scanSpeed);
  ultrasonic();
  sector_distance[i] = distance_sm;   
  }
  
 // второй проход
 for ( byte i = 5; i >0; i--){
  USMMotor.write(100-60+(i-1)*30);
  delay(scanSpeed);
  ultrasonic();
  sector_distance[i] = (distance_sm+sector_distance[i])/2;   
  }

  for(byte i = 0; i<5; i++) {
    Serial.print(sector_distance[i]);
    Serial. print("|");
  }
  
 
  Serial.print("\n");
  USMMotor.write(100);
}

// поворот машинки
void rotate(byte dir){
  
  int lms = left_Servo_Zero_Point;
  int rms = right_Servo_Zero_Point;
  
  if (!motorSinc) {
    
    if(dir>2){ //надо повернуть влево
    
    lms -= dir*dir; 
    rms -= dir*dir; 
    
    }    
    if(dir<2){ // надо повернуть вправо
    lms += 16-dir*7; 
    rms += 16-dir*7;
    }
  }

 // крутим моторчики некоторое время 
  
  LeftServoMotor.write(lms);
  RightServoMotor.write(rms);
  delay(500);
  LeftServoMotor.write(left_Servo_Zero_Point);
  RightServoMotor.write(right_Servo_Zero_Point);
  
}
//

// движение
void move_forvard(){
  
  int lms;
  int rms;
  if (!motorSinc) {
    lms = left_Servo_Zero_Point + w_speed;
    rms = right_Servo_Zero_Point - w_speed;
  }
  else {   
    lms= left_Servo_Zero_Point - w_speed;
    rms= right_Servo_Zero_Point - w_speed;  
  }

  // крутим моторчики  
  LeftServoMotor.write(lms);
  RightServoMotor.write(rms);
}


/* замер расстояния */
ISR(TIMER2_COMPA_vect) {
}

void ultrasonic(){
  
  if(dist_m){
  dist_m = false; // сбрасываем флаг подтверждения расстояния
  PORTB |= 1<<trig_Pin;
  delayMicroseconds(10);
  PORTB &= ~(1<<trig_Pin);
  startMks = micros();
  }
}

// вычисляем задержку по спаду эха
ISR(PCINT0_vect ){
  //cli();
  stopMks = micros();
  PORTB ^= 1<<led_pin; 
  distance_sm=(stopMks-startMks)/(29*2);
  dist_m= true;
  //sei();
}


/**/




void serial_print(){
  
  Serial.print("TCNT2 ");
  Serial.print(TCNT2);
  Serial.print("\n");

  //  byte xx[]={OCR1AL,OCR1AH};
  //  unsigned int *y = (unsigned int *)&xx;
  //  Serial.print("OCR1A");
  //  Serial.print(y[0]);
  Serial.print("OCR2A "); 
  Serial.print(OCR2A);
  Serial.print("\n");

  Serial.print("OCR2B "); 
  Serial.print(OCR2B);
  Serial.print("\n");

  Serial.print("TCCR2A ");
  Serial.print(TCCR2A);
  Serial.print("\n");

  Serial.print("TCCR2B ");
  Serial.print(TCCR2B);
  Serial.print("\n");


  //  PRR |=1<<PRTIM0; // запрет таймера Т1
  //  PRR |=1<<PRTIM2; // запрет таймера Т2

  Serial.print("EICRA "); 
  Serial.print(EICRA);
  Serial.print("\n");

  Serial.print("PCICR "); 
  Serial.print(PCICR);
  Serial.print("\n");

  delay(1000);
  if (Serial.available()){

  Serial.print("\n");
  Serial.print("\n");
  Serial.print("\n");
  
  }
}

Как видите все работает. Разбирайтесь. По писал сын -10 лет, корпус резал я.

Сыну огромное спасибо за ПО которое он написал.Да и корпус не плохо сделан. Получилось у вас очень не плохо.

мне бы не плохо было разбраться бы со своим кодом. меня сейчас больше всего интересует как программно по очереди запускать серво мотор , ультразвуковой  и  делать проверку местности.