Katedra Transportu Kolejowego Wydział Transportu i Inżynierii Lotniczej

				
					//Mechatronika - robot 4WD mecanum - testowanie silników
//Adam Mańka, adam.manka(znak)polsl.polsl
void setup() {
  //Podłączenia silników do Arduino UNO przez modół mostka H L298N
  pinMode(12, OUTPUT); //Prawy przód 
  pinMode(13, OUTPUT); //Prawy przód 
  pinMode( 8, OUTPUT); //Prawy tył 
  pinMode(11, OUTPUT); //Prawy tył 
  pinMode( 2, OUTPUT); //Lewy Przód 
  pinMode( 4, OUTPUT); //Lewy Przód
  pinMode( 6, OUTPUT); //Lewy Tył 
  pinMode( 7, OUTPUT); //Lewy Tył
}

void loop() {
  //Czekamy przez 1 sekundę z wyłączonymi silnikami
  stop(true);
  delay(1000);           

  //Obracamy kołami do przodu przez 1 sekundę
  //Można uruchomić tylko po jednym silniku lub wszystkie
  //razem po skasowaniu delay
  prawyPrzod(true);
  delay(1000);
  prawyTyl(true);
  delay(1000);
  lewyPrzod(true);
  delay(1000);
  lewyTyl(true);
  delay(1000);          
  
  //Obracamy kołami do tyłu przez 2 sekundy
  prawyPrzod(false);
  prawyTyl(false);
  lewyPrzod(false);
  lewyTyl(false);
  delay(2000);        
}

void prawyPrzod (bool k) {
digitalWrite(12, !k); //Zmiana kierunku - > albo zmiana kabelków miejscami albo zmiana w kodzie miejsca z negacją !
digitalWrite(13, k);
}

void prawyTyl (bool k) {
digitalWrite( 8, k);
digitalWrite(11, !k);
}

void lewyPrzod (bool k) {
digitalWrite( 2, !k); 
digitalWrite( 4, k);
}

void lewyTyl (bool k) {
digitalWrite( 6, !k); 
digitalWrite( 7, k);
}

void stop (bool k) {
digitalWrite( 2, LOW);
digitalWrite( 4, LOW);
digitalWrite( 6, LOW);
digitalWrite( 7, LOW);
digitalWrite( 8, LOW);
digitalWrite(11, LOW);
digitalWrite(12, LOW);
digitalWrite(13, LOW);
}