Arduino.ru

Всем привет!

Есть проблемка с движками. Помогите решить, пожалуйста.

При движении вперед движки работают только на максимальной скорости (255);" при других скоростях оба канала не работают. При движении назад работает только один канал и, также, только на максимальной скорости.

Участники:

1. Arduino UNO R3

2. Sensor shield

3. Stepper Motor Driver L298 

4. 4x DC motors на 5V

К плате подключены также: серва TowerPro 9G, сонар Ultrasonic HC-SR04, IR приемник (эти девайсы работают нормально). 

Схема работает одинаково неправильно при разных вариантах питания (раздельное с разной землей, раздельное с общей землей, монопольное). На движках стоят 104 кондеры (по три на каждом). Ниже привожу фотку драйвера и код:

Перемычки U1, U2, U3, U4, 5V_EN убраны

#include <Servo.h>
#include <IRremote.h>
#include <NewPing.h>


// ULTRASONIC PINS
#define TRIGGER_PIN   13  // yellow
#define ECHO_PIN      12  // orange
#define MAX_DISTANCE  500

#define MIN_DISTANCE  26

// SERVO PINS
#define SERVO_PIN     7 // orange

// IR PINS
#define IR_PIN        11 // yellow

// MOTOR SHIELD PINS
#define ENA_PIN       9 // black
#define ENB_PIN       10 // white
#define IN1_PIN       2  // gray     BACK
#define IN2_PIN       3  // violet   FORWARD
#define IN3_PIN       4  // blue     FORWARD
#define IN4_PIN       5  // green    BACK

// BLUETOTH PINS
#define RX_PIN        0  // yellow
#define TX_PIN        1  // orange


boolean OPERATION = true;
unsigned int DISTANCE = 0;
// boolean LDir = true, RDir = true; // true = Forward, false = Backward

boolean Changes = false;

boolean DIR = true;
int SPEED = 255;

// --------------------------------------------------------------------------------

NewPing            ultrasonic(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

Servo              servoDrive;


IRrecv             ir(IR_PIN);
decode_results     irResult;


int getAngle(void);
int getAngleArray(void);
unsigned int getDistance(void);

void TURNFB(boolean, int);
void TURNLR(int);
void MOVE(boolean, unsigned int);
void STOP(void);


// ---------------------------------------------------------------------------------

// ----------------------- SETUP BEGIN ---------------------------------------------
void setup() {
  Serial.begin(9600);
  servoDrive.attach(SERVO_PIN);
  servoDrive.write(90);
  delay(200);
  servoDrive.detach();
  
  ir.enableIRIn();                   // IR Initialisation
  
                                     // MOTOR Initialisation
  pinMode(IN1_PIN, OUTPUT);                // ---- Dir 1 ---------
  pinMode(IN2_PIN, OUTPUT);                // ---- Dir 2 ---------
  pinMode(ENA_PIN, OUTPUT);               // ---- ENA -----------
  
  pinMode(IN3_PIN, OUTPUT);                // ---- Dir 1 ---------
  pinMode(IN4_PIN, OUTPUT);                // ---- Dir 2 ---------
  pinMode(ENB_PIN, OUTPUT);               // ---- ENB -----------
  
    

}
// ------------------------- END SETUP ---------------------------------------------


// ------------------------ LOOP BEGIN ---------------------------------------------
void loop() {
  if (ir.decode(&irResult)) {
    switch (irResult.value) {
      case 0xFF02FD:
        if (OPERATION == false) OPERATION = true; else OPERATION = false; 
        
        Changes = true;
        
        Serial.print ("OPERATION = ");
        Serial.print (OPERATION);
        Serial.println ();
      break;
      // ---------------------- oter cases -----------------------------------------
      case 0xFF629D:
        DIR = true;
        Changes = true;
        
        Serial.println ("FORWARD DIRECTION");
      break;
      
      case 0xFFA857:
        DIR = false;
        Changes = true;
        
        Serial.println ("BACKWARD DIRECTION");
      break;
      
      case 0xFF42BD:
        if (SPEED >= 5) {
          SPEED -= 5;
          Changes = true;
          
          Serial.print ("SPEED = ");
          Serial.println (SPEED);
        }
      break;
      
      case 0xFF52AD:
        if (SPEED <= 250) {
          SPEED += 5;
          Changes = true;
          
          Serial.print ("SPEED = ");
          Serial.println (SPEED);
        }
      break;
      // ---------------------- oter cases -----------------------------------------
    }
  ir.resume();
  }
  //delay(100);
  //Serial.println ("loop");
  DISTANCE = getDistance();
  
  if (DISTANCE < MIN_DISTANCE) Changes = true;
  
  if (Changes == true) MOVE(OPERATION, DISTANCE);
 
}
// ------------------------ END LOOP -----------------------------------------------


void STOP() {
  analogWrite (ENA_PIN, 0);
  digitalWrite (IN1_PIN, LOW);
  digitalWrite (IN2_PIN, LOW);
  
  analogWrite (ENB_PIN, 0);
  digitalWrite (IN3_PIN, LOW);
  digitalWrite (IN4_PIN, LOW);
}

void TURNFB(boolean Dir, int Speed) {`
    //delay (100);
    analogWrite (ENA_PIN, Speed);
    analogWrite (ENB_PIN, Speed);
    //delay (500);
    if (Dir == true) {
      digitalWrite (IN1_PIN, LOW);
      digitalWrite (IN2_PIN, HIGH); //BACKWARD
      digitalWrite (IN3_PIN, HIGH); //BACKWARD
      digitalWrite (IN4_PIN, LOW);
    } else {
      digitalWrite (IN1_PIN,HIGH);
      digitalWrite (IN2_PIN, LOW); //BACKWARD
      digitalWrite (IN3_PIN, LOW); //BACKWARD
      digitalWrite (IN4_PIN, HIGH);
    }
}

void TURNLR(int angle) {
  // "TURN CAR LEFT OR RIGHT
  return;
} 

void MOVE(boolean operation, unsigned int distance) {
  
  Serial.print ("TURNF FUNCTION:");
  Serial.print (" OPERATION = ");
  Serial.print (operation);
  Serial.print (" DISTANCE = ");
  Serial.println (distance);
  
  if (operation == false) {
    STOP();
            
    Serial.println ("STOP");    
    
    Changes = false;
    return;
  }
    
  if (operation == true && (distance > MIN_DISTANCE || distance == 0)) {
         
    Serial.println ("MOVE FORWARD");
    
    TURNFB(DIR, SPEED);
    
    Changes = false;
    return;
  }
  
  if (operation == true && distance <= MIN_DISTANCE) {
    
    Serial.println ("COLLISION EMEREGENCY");
    
    STOP();
  
    // int optangle = getAngleArray();// FIND NEW DIRECTION !!!!!!!
  
    TURNLR(getAngleArray());
  
    Changes = false;
    return;
  }  
}

int getAngleArray() {
  servoDrive.attach(SERVO_PIN);
  unsigned int Angles[18] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  unsigned int rDistance = 1;
  int rangle = 90;
  for (int a = 0; a <= 18; a++) {
    servoDrive.write(a * 10);
    //delay(50);
    Angles[a] = getDistance();
    Serial.print ("Angle = ");
    Serial.print (a * 10);
    Serial.print (" Distance = ");
    Serial.println (Angles[a]);
  }
  
  for (int a = 0; a <= 18; a++) {
    if (Angles[a] > rDistance) {
      rDistance = Angles[a];
      rangle = a * 10;
    }  
  }
  
  servoDrive.write(90);
  delay(200);
  Serial.print ("OPTIMAL ANGLE = ");
  Serial.print (rangle);
  Serial.print (" MAX DISTANCE = ");
  Serial.println (rDistance);
  servoDrive.detach();
    
  if (rangle < 90) return -(90 - rangle);
  if (rangle > 90) return rangle - 90;
  if (rangle == 90) return 0;
  
}

unsigned int getDistance() {
  delay(200);
  unsigned int uS = ultrasonic.ping(); // Send ping, get ping time in microseconds (uS).
  
  //Serial.print ("getDistance = ");
  //Serial.println (uS / US_ROUNDTRIP_CM);
  
  return (uS / US_ROUNDTRIP_CM);
 
}