Robotas voras 2

Antra roboto voro versija papildyta kliūties davikliais. Dabar žaislas pats apeina kliūtis į jas neatsitrenkdamas.
Pirminė schemos versija papildyta dviem IR diodais ir indikaciniais diodais, išmestas įtampos stabilizatorius, nebenaudojamas kvarcinis rezonatorius, ATMEGA328P taktuojama nuo vidinio 8MHz generatoriaus.
Taktinio dažnio sumažinimas atsiliepia signalo servovarikliams tikslumui. Pertraukimas aptarnaujantis IR imtuvą stabdo taimerį valdantį servovariklius. Dėl neišlaikomo pastovaus servovariklių valdymo impulso pločio, kelis kartus per minutę, atsiranda papildomi servovariklių judesiai. Kas primena žaislinio voro traukulius, kurie aiškiai matomi tik esant žaislui ramybės būsenoje ar naudojant su žaislu nesuderintą pultelį toje pačioje patalpoje.

Detalių sąrašas

EAGLE Version 5.12.0 Copyright (c) 1988-2011 CadSoft
Part Value Device Package Library Sheet
 C1 220pF C-EU050-025X075 C050-025X075 rcl 1C2 47uF CPOL-EUE2.5-7 E2,5-7 rcl 1
 C3 47uF CPOL-EUE2.5-7 E2,5-7 rcl 1
 D1 1N4004 1N4004 DO41-10 diode 1
 IC1 MEGA48/88/168-PU MEGA48/88/168-PU DIL28-3 avr-7 1
 IC2 TSOP34838 TSOP34838 TSOP348 vishay-1 1
 INFO LED5MM LED5MM led 1
 IR1 LED5MM LED5MM led 1
 IR2 LED5MM LED5MM led 1
 AKIS1 LED5MM LED5MM led 1
 AKIS2 LED5MM LED5MM led 1
 POWER LED5MM LED5MM led 1
 JP1 AVR-ISP-6VERT AVR-ISP-6VERT AVR-ISP-6 avr-7 1
 R1 1k5 R-EU_0204/7 0204/7 rcl 1
 R2 1k5 R-EU_0204/7 0204/7 rcl 1
 R3 1k R-EU_0204/7 0204/7 rcl 1
 R4 1k R-EU_0204/7 0204/7 rcl 1
 R5 1k R-EU_0204/7 0204/7 rcl 1
 R6 100 R-EU_0204/7 0204/7 rcl 1
 R7 1k R-EU_0204/7 0204/7 rcl 1
 SV1 MA03-1 MA03-1 con-lstb 1
 SV2 MA03-1 MA03-1 con-lstb 1
 SV3 MA03-1 MA03-1 con-lstb 1

Schema

schema

 

Spausdintinė plokštėje

isdestymas

Detalių išdėstymas plokštėje

plokste

Programos kodas

#include
#include
#define pause 13
#define impulsai 16
int RECV_PIN = 2;
IRrecv irrecv(RECV_PIN);
decode_results results;
Servo servok; //kairė
Servo servoc; //centras
Servo servod; //desinė

int c = 90; //variklių centrinė padėtis
int z = 5; //pasukimo žingsnis
int koj = -25; //kojos kėlimo aukštis
int p = 100; //pauzės trukmė
int k = 0; //judėjimo kryptis
int ksena = 0; //senoji kryptis
int mpauz; //minimali pauzė
int kof = 35; //sukimo greitis ms/laipsniui*10
unsigned long rez; //ir dekoderio rezultatas
boolean kair, desn;
boolean kliutis = false; //kliūties daviklių rezultatai

int kr[18][6];
/* servo mechanizmų judėjimo tvarka
0 - nedarom nieko

pasukimai su pauze
 1 - kairio mechanizmo pasukimo kampas (c + z)
 2 - centrinio mechanizmo pasukimo kampas (c + koj)
 3 - dešinio mechanizmo pasukimo kampas (c + z)
 -1 - kairio mechanizmo pasukimo kampas (c - z)
 -2 - centrinio mechanizmo pasukimo kampas (c - koj)
 -3 - dešinio mechanizmo pasukimo kampas (c - z)
 4 - kairio mechanizmo pasukimo kampas (c)
 5 - centrinio mechanizmo pasukimo kampas (c)
 6 - dešinio mechanizmo pasukimo kampas (c)

pasukimai be pauzės
 11 - kairio mechanizmo pasukimo kampas (c + z)
 12 - centrinio mechanizmo pasukimo kampas (c + koj)
 13 - dešinio mechanizmo pasukimo kampas (c + z)
 -11 - kairio mechanizmo pasukimo kampas (c - z)
 -12 - centrinio mechanizmo pasukimo kampas (c - koj)
 -13 - dešinio mechanizmo pasukimo kampas (c - z)
 14 - kairio mechanizmo pasukimo kampas (c)
 15 - centrinio mechanizmo pasukimo kampas (c)
 16 - dešinio mechanizmo pasukimo kampas (c) */

void setup()
 {
   pinMode(4, OUTPUT);
   pinMode(5, OUTPUT);
   pinMode(6, OUTPUT);
   pinMode(7, OUTPUT);
   pinMode(13, OUTPUT);
   kr = {
    { 14,15,16,0,0,0 }, //stop
    { 2,11,3,-2,-11,-3 }, //pirmyn
    { -2,11,3,2,-11,-3 }, //atgal
    { 2,11,-3,-2,-11,3 }, //kairėn
    { 2,-11,3,-2,11,-3 }, //dešinėn
    { 4,2,6,4,-2,6 }, //kilnojam kojas
    { 12,3,-3,0,0,0 }, //mojam1
    { -12,1,-1,0,0,0 }, //mojam2
    { 11,-3,-11,3,0,0 }, //makaluojamės
    { 13,-11,0,0,0,0 }, //stop kojos priekin
    { -13,11,0,0,0 }, //stop kojos atgal
    { 2,0,0,0,0,0 }, //keliam kairę pusę
    { -2,0,0,0,0,0 }, //keliam dešinę pusę
    { 5,0,0,0,0,0 }, //nusileidžiam
    { 3,0,0,0,0,0 }, //kairė koja priekin
    { -3,0,0,0,0,0 }, //kairė koja atgal
    { -1,0,0,0,0,0 }, //dešinė koja priekin
    { 1,0,0,0,0,0 } //dešinė koja atgal
   };
   irrecv.enableIRIn();
   servok.attach(9);
   servoc.attach(10);
   servod.attach(11);
   k = 0;
   ksena = 0;
   rez = 41;
 }
void akys() {
  int i,n;
  servok.detach();
  servoc.detach();
  servod.detach();
  cli();
  for (i=0; i<impulsai; i++) {
    digitalWrite(4, HIGH);
    delayMicroseconds(pause);
    digitalWrite(4, LOW);
    delayMicroseconds(pause);
  }
  kair = !(PIND & (1 << 2));
  for (i=0; i<impulsai; i++) {
    digitalWrite(5, HIGH);
    delayMicroseconds(pause);
    digitalWrite(5, LOW);
    delayMicroseconds(pause);
  }
  desn = !(PIND & (1 << 2)); 
  digitalWrite(7, kair); 
  digitalWrite(6, desn); 
  sei(); 
  servok.attach(9); 
  servoc.attach(10); 
  servod.attach(11); 
  kliutis = desn || kair || kliutis; 
}

void judam() { 
  int kk,pp; 
  if (z > koj) 
    mpauz = kof * z / 10;
  else 
    mpauz = kof * koj / 10;
  for (int i=0; i < 6; i++) { 
    pp = 1; 
  kk = kr[k][i]; 
  if (kk >= 10) {
    kk = kk - 10;
    pp = 0;
  }
  if (kk <= -10) {
    kk = kk + 10;
    pp = 0;
  }
  switch ( kk ) {
    case 0: pp = 0; break;
    case 1: servok.write(c+z); break;
    case -1:  servok.write(c-z); break;
    case 2: servoc.write(c+koj); break;
    case -2: servoc.write(c-koj); break;
    case 3: servod.write(c+z); break;
    case -3: servod.write(c-z); break;
    case 4: servok.write(c); break;
    case 5: servoc.write(c); break;
    case 6: servod.write(c); break;
  }
  if (pp == 1) delay(p + mpauz);
  }
}

void korekcija() {
  if ( kair && desn ) {
    ksena = k;
    k = 2;
  }
  if ( !kair && desn ) {
    ksena = k;
    k = 4;
  }
  if ( kair && !desn ) {
    ksena = k;
    k = 3;
  }
}

void loop() {
 if (irrecv.decode(&results)) {
   digitalWrite(13, HIGH);
   delay(3);
   rez = results.value;
   irrecv.resume();
   digitalWrite(13, LOW);
 } else {
   switch (k) {
     case 1:
     case 3:
     case 4:
     akys();
  }
 }
 switch (rez) {
   case 788727: k = 0; ksena = 0;  kliutis = false; break;
   case 839727: k = 1; break;
   case 798927: k = 2; break;
   case 831567: k = 3; break;
   case 815247: k = 4; break;
   case 803007: k = 5; break;
   case 835647: k = 6; break;
   case 847887: k = 7; break;
   case 843807: k = 11; break;
   case 975847: k = 12; break;
   case 827487: k = 14; break;
   case 811167: k = 15; break;
   case 828507: k = 16; break;
   case 812187: k = 17; break;
   case 792807: if (p <= 500) p += 50; break; 
   case 825447: if (p >= 50) p -= 50; break;
   case 803262: if (z < 20) z += 2; break; 
   case 835902: if (z > 2) z -= 2; break;
 }
 if ( kliutis ) korekcija();
 digitalWrite(7, LOW);
 digitalWrite(6, LOW);
 if ((!kair && !desn) && kliutis ) {
   k = ksena;
   kliutis = false;
 }
 judam();
}

1 komentaras

Parašykite komentarą

El. pašto adresas nebus skelbiamas. Būtini laukeliai pažymėti *