Robot s mecanum podvozkom

Zo stránky Robotický krúžok
Skočit na navigaci Skočit na vyhledávání

Odkazy: 

PWMA  11    10
AIN2  3     8
AIN1  A1    5
BIN1  2     4
BIN2  7     A0
PWMB  6     9
  • Prvá pekná funkčná verzia:
void setup() {
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(14, OUTPUT);
  pinMode(15, OUTPUT);
  Serial.begin(9600);
}

// IN1: H, IN2: L, PWM: H
void left_motor_a_cw() {
  digitalWrite(15, HIGH);
  digitalWrite(3, LOW);
  digitalWrite(11, HIGH);
}

void left_motor_a_ccw() {
  digitalWrite(3, HIGH);
  digitalWrite(15, LOW);
  digitalWrite(11, HIGH);
}


void left_motor_b_cw() {
  digitalWrite(2, HIGH);
  digitalWrite(7, LOW);
  digitalWrite(6, HIGH);
}

void left_motor_b_ccw() {
  digitalWrite(7, HIGH);
  digitalWrite(2, LOW);
  digitalWrite(6, HIGH);
}

void right_motor_a_cw() {
  digitalWrite(5, HIGH);
  digitalWrite(8, LOW);
  digitalWrite(10, HIGH);
}

void right_motor_a_ccw() {
  digitalWrite(8, HIGH);
  digitalWrite(5, LOW);
  digitalWrite(10, HIGH);
}

void right_motor_b_cw() {
  digitalWrite(4, HIGH);
  digitalWrite(14, LOW);
  digitalWrite(9, HIGH);
}

void right_motor_b_ccw() {
  digitalWrite(14, HIGH);
  digitalWrite(4, LOW);
  digitalWrite(9, HIGH);
}

void stop_all_motors() {
  // L: 2,3,4,5,6,8,9,15
  // H: 7,10,11,14
  digitalWrite(2, LOW);
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(5, LOW);
  digitalWrite(7, LOW);
  digitalWrite(8, LOW);
  digitalWrite(14, LOW);
  digitalWrite(15, LOW);

  digitalWrite(6, HIGH);
  digitalWrite(10, HIGH);
  digitalWrite(11, HIGH);
  digitalWrite(9, HIGH);
}

// 0  toc vlavo
// 3  dopredu
// 6  vlavo
// 9  vpravo
// 12  dozadu
// 15  toc vpravo

void otocka_vlavo()
{
    left_motor_a_ccw();
    left_motor_b_ccw();
    right_motor_a_ccw();
    right_motor_b_ccw();
}

void otocka_vpravo()
{
    left_motor_a_cw();
    left_motor_b_cw();
    right_motor_a_cw();
    right_motor_b_cw();
}

void vzad()
{
    left_motor_a_ccw();
    left_motor_b_ccw();
    right_motor_a_cw();
    right_motor_b_cw();
}

void vpred()
{
    left_motor_a_cw();
    left_motor_b_cw();
    right_motor_a_ccw();
    right_motor_b_ccw();
}

void vpravo()
{
      left_motor_a_ccw();
    left_motor_b_cw();
    right_motor_a_cw();
    right_motor_b_ccw();
}

void vlavo()
{
      left_motor_a_cw();
    left_motor_b_ccw();
    right_motor_a_ccw();
    right_motor_b_cw();
}

void loop() {

  //experiment 1

  //vzad
    vzad();
    delay(1000);
    stop_all_motors();
    delay(1000);

  //vpred
    vpred();
    delay(1000);
    stop_all_motors();
    delay(1000);

  //otocka vlavo
    otocka_vlavo();
    delay(1000);
    stop_all_motors();
    delay(1000);

  //otocka vpravo
    otocka_vpravo();
    delay(1000);
    stop_all_motors();
    delay(1000);

  //vlavo
    vlavo();
    delay(1000);
    stop_all_motors();
    delay(1000);

  //vpravo
    vpravo();
    delay(1000);
    stop_all_motors();
    delay(1000);
}

Chceli by sme funkciu, ktorá pohne vozíkom do zadaného uhla zadanou rýchlosťou. Prvý pokus: 

void sikmo_uhol_vpravo(int angle, int speed)
{
  if (uhol < 45)
  {
    int s2 = round(speed * tan((45 - angle) / 180.0 * M_PI));
    left_motor_a_cw(speed);
    left_motor_b_cw(s2);
    right_motor_a_cw(speed);
    right_motor_b_cw(s2);
  }
  else   if (uhol < 90)
  {
     int s2 = round(speed * tan((angle - 45) / 180.0 * M_PI))
    left_motor_a_cw(speed);
    left_motor_b_ccw(s2);
    right_motor_a_ccw(speed);
    right_motor_b_cw(s2);
  }
  else   if (uhol < 135)
  {
       int s2 = round(speed * tan((angle- 45) / 180.0 * M_PI))
    left_motor_a_cw(s2);
    left_motor_b_ccw(speed);
    right_motor_a_ccw(s2);
    right_motor_b_cw(speed);
  }
  else
  {
     int s2 = round(speed * tan((angle - 135) / 180.0 * M_PI))
    left_motor_a_ccw(s2);
    left_motor_b_ccw(speed);
    right_motor_a_ccw(s2);
    right_motor_b_ccw(speed);
  }
}

Pokus o program: 

#include <Adafruit_NeoPixel.h>
#define PIN 13 // Pin connected to the Neopixel strip
#define NUMPIXELS 3 // Number of pixels in the strip

Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);

#include <math.h>
void setup() {
  pixels.begin();
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(14, OUTPUT);
  pinMode(15, OUTPUT);
  Serial.begin(9600);
}


void farba(uint32_t f) {
    pixels.setPixelColor(1, f);
    pixels.setPixelColor(2, f);
    pixels.setPixelColor(0, f);
    pixels.setBrightness(255); // 50% brightness
    pixels.show();
}

// IN1: H, IN2: L, PWM: H
void left_motor_a_cw(int speed) {
  digitalWrite(15, HIGH);
  digitalWrite(3, LOW);
  analogWrite(11, speed);
}

void left_motor_a_ccw(int speed) {
  digitalWrite(3, HIGH);
  digitalWrite(15, LOW);
  analogWrite(11, speed);
}


void left_motor_b_cw(int speed) {
  digitalWrite(2, HIGH);
  digitalWrite(7, LOW);
  analogWrite(6, speed);
}

void left_motor_b_ccw(int speed) {
  digitalWrite(7, HIGH);
  digitalWrite(2, LOW);
  analogWrite(6, speed);
}

void right_motor_a_cw(int speed) {
  digitalWrite(5, HIGH);
  digitalWrite(8, LOW);
  analogWrite(10, speed);
}

void right_motor_a_ccw(int speed) {
  digitalWrite(8, HIGH);
  digitalWrite(5, LOW);
  analogWrite(10, speed);
}

void right_motor_b_cw(int speed) {
  digitalWrite(4, HIGH);
  digitalWrite(14, LOW);
  analogWrite(9, speed);
}

void right_motor_b_ccw(int speed) {
  digitalWrite(14, HIGH);
  digitalWrite(4, LOW);
  analogWrite(9, speed);
}

void stop_all_motors() {
  // L: 2,3,4,5,6,8,9,15
  // H: 7,10,11,14
  digitalWrite(2, LOW);
  digitalWrite(3, LOW);
  digitalWrite(4, LOW);
  digitalWrite(5, LOW);
  digitalWrite(7, LOW);
  digitalWrite(8, LOW);
  digitalWrite(14, LOW);
  digitalWrite(15, LOW);

  digitalWrite(6, HIGH);
  digitalWrite(10, HIGH);
  digitalWrite(11, HIGH);
  digitalWrite(9, HIGH);
}


  /*
  //verzia testu 1

  left_motor_a_cw();
  left_motor_b_cw();
  right_motor_a_cw();
  right_motor_b_cw();
  delay(2500);
  stop_all_motors();
  delay(2500);
 
  left_motor_a_ccw(255);
  left_motor_b_ccw();
  right_motor_a_ccw();
  right_motor_b_ccw();
  delay(2500);
stop_all_motors();
  delay(2500);
 
  left_motor_a_cw();
  left_motor_b_cw();
  right_motor_a_ccw();
  right_motor_b_ccw();
  delay(2500);
stop_all_motors();
  delay(2500);
 
  left_motor_a_ccw(255);
  left_motor_b_ccw();
  right_motor_a_cw();
  right_motor_b_cw();
  delay(2500);
stop_all_motors();
  delay(2500);
 
  left_motor_a_ccw(255);
  left_motor_b_ccw();
  right_motor_a_ccw();
  right_motor_b_cw();
  delay(2500);
*/

// 0  toc vlavo
// 3  dopredu
// 6  vlavo
// 9  vpravo
// 12  dozadu
// 15  toc vpravo

void otocka_vlavo(int speed)
{
    left_motor_a_ccw(speed);
    left_motor_b_ccw(speed);
    right_motor_a_ccw(speed);
    right_motor_b_ccw(speed);
}

void otocka_vpravo(int speed)
{
    left_motor_a_cw(speed);
    left_motor_b_cw(speed);
    right_motor_a_cw(speed);
    right_motor_b_cw(speed);
}


void sikmo_vpravo(int speed1, int speed2)
{
    left_motor_a_cw(speed1);
    left_motor_b_ccw(speed2);
    right_motor_a_ccw(speed1);
    right_motor_b_cw(speed2);
}

//sikma vpravo

void sikmo_uhol_vpravo(int angle, int speed)
{
  if (angle < 45)
  {
    int s2 = round(speed * tan((45 - angle) / 180.0 * M_PI));
    left_motor_a_cw(speed);
    left_motor_b_cw(s2);
    right_motor_a_cw(speed);
    right_motor_b_cw(s2);
  }
  else   if (angle < 90)
  {
     int s2 = round(speed * tan((angle - 45) / 180.0 * M_PI));
    left_motor_a_cw(speed);
    left_motor_b_ccw(s2);
    right_motor_a_ccw(speed);
    right_motor_b_cw(s2);
  }
  else   if (angle < 135)
  {
       int s2 = round(speed * tan((angle- 45) / 180.0 * M_PI));
    left_motor_a_cw(s2);
    left_motor_b_ccw(speed);
    right_motor_a_ccw(s2);
    right_motor_b_cw(speed);
  }
  else
  {
     int s2 = round(speed * tan((angle - 135) / 180.0 * M_PI));
    left_motor_a_ccw(s2);
    left_motor_b_ccw(speed);
    right_motor_a_ccw(s2);
    right_motor_b_ccw(speed);
  }


}

//sikma vlavo

void sikmo_uhol_vlavo(int angle, int speed)
{
  if (angle < 45)
  {
    int s2 = round(speed * tan((45 - angle) / 180.0 * M_PI));
    left_motor_b_cw(speed);
    left_motor_a_cw(s2);
    right_motor_b_cw(speed);
    right_motor_a_cw(s2);
  }
  else   if (angle < 90)
  {
     int s2 = round(speed * tan((angle - 45) / 180.0 * M_PI));
    left_motor_b_cw(speed);
    left_motor_a_ccw(s2);
    right_motor_b_ccw(speed);
    right_motor_a_cw(s2);
  }
  else   if (angle < 135)
  {
       int s2 = round(speed * tan((angle- 45) / 180.0 * M_PI));
    left_motor_b_cw(s2);
    left_motor_a_ccw(speed);
    right_motor_b_ccw(s2);
    right_motor_a_cw(speed);
  }
  else
  {
     int s2 = round(speed * tan((angle - 135) / 180.0 * M_PI));
    left_motor_b_ccw(s2);
    left_motor_a_ccw(speed);
    right_motor_b_ccw(s2);
    right_motor_a_ccw(speed);
  }


}

//otocka pokus

void pokus(int speed)
{
  for (int a = 0; a < 180; a = a + 20)
  {
    farba(pixels.Color(a * 7 / 5, 0, 255 - a * 7 / 5));
    sikmo_uhol_vpravo(a, speed);
    delay(3000);
    stop_all_motors();
    delay(3000);
    
  }
  for (int a = 180; a > 0; a = a - 20)
  {
    farba(pixels.Color(255 - a * 7 / 5, 0, a * 7 / 5));
    sikmo_uhol_vlavo(a, speed);
    delay(3000);
    stop_all_motors();
    delay(3000);
    
  }
  stop_all_motors();
}

void vzad(int speed)
{
    left_motor_a_ccw(speed);
    left_motor_b_ccw(speed);
    right_motor_a_cw(speed);
    right_motor_b_cw(speed);
}

void vpred(int speed)
{
    left_motor_a_cw(speed);
    left_motor_b_cw(speed);
    right_motor_a_ccw(speed);
    right_motor_b_ccw(speed);
}

void vpravo(int speed)
{
      left_motor_a_ccw(speed);
    left_motor_b_cw(speed);
    right_motor_a_cw(speed);
    right_motor_b_ccw(speed);
}

void vlavo(int speed)
{
    left_motor_a_cw(speed);
    left_motor_b_ccw(speed);
    right_motor_a_ccw(speed);
    right_motor_b_cw(speed);
}

void loop() {

  pokus(220);

  //experiment 1

  sikmo_vpravo(255, 110);
  delay(3000);
  stop_all_motors();
  delay(1000);

  return;

  for (int s = 127; s < 256; s += 32)
  {
  //vzad
    vzad(s);
    delay(1000);
    stop_all_motors();
    delay(1000);

  //vpred
    vpred(s);
    delay(1000);
    stop_all_motors();
    delay(1000);

  //otocka vlavo
    otocka_vlavo(s);
    delay(1000);
    stop_all_motors();
    delay(1000);

  //otocka vpravo
    otocka_vpravo(s);
    delay(1000);
    stop_all_motors();
    delay(1000);

  //vlavo
    vlavo(s);
    delay(1000);
    stop_all_motors();
    delay(1000);

  //vpravo
    vpravo(s);
    delay(1000);
    stop_all_motors();
    delay(1000);
  }
}


/*


//verzia testu 2

  for (int i = 0; i < 16; i++) {
    if (i & 1) left_motor_a_cw();
    else left_motor_a_ccw(255);
    if (i & 2) left_motor_b_cw();
    else left_motor_b_ccw();
    if (i & 4) right_motor_a_cw();
    else right_motor_a_ccw();
    if (i & 8) right_motor_b_cw();
    else right_motor_b_ccw();
    delay(2500);
    stop_all_motors();
    delay(2500);


  }


  stop_all_motors();
  delay(5000);
}

*/
Zdroj: „https://kruzok.net/index.php?title=Robot_s_mecanum_podvozkom&oldid=2759“