Robot s mecanum podvozkom

Odkazy:

• datasheet k radiču na motory
• -dvadsiata verzia programu

PWMA  11    10
AIN2  3     8
AIN1  A1    5
BIN1  2     4
BIN2  7     A0
PWMB  6     9

• testovací program pre všetkých 16 kombinácií

• 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);
}

• Stiahnite si program!

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);
  }
}