Thiết kế website giá rẻ

Question

As a school project I was tasked with creating a 2WD car powered by arduino and will use a controlling software on pc that connects them together using bluetooth (Specifically the HC-06 module). The tests were going great I was making sure the motors, software, connection were working properly but now when I try to move my car any other direction excluding straight my program crashes. This may be a result of poor programming on the Arduino side but I still wanted to ask here as I might have made some mistakes in the Python side aswell. I’m providing codes for both of them below:

When I press W on the keyboard there is no problem but when I try to give other inputs such as A and D the app crashes and the car isn’t able to take any more inputs.

import pygame
import serial


ser = serial.Serial("COM3", 9600, timeout= 5)


pygame.init()
pygame.font.init()
FONT = pygame.font.SysFont("Arial", 30)


WINDOW_SIZE = 800
window = pygame.display.set_mode((WINDOW_SIZE, WINDOW_SIZE))
pygame.display.set_caption('TOGG 1X Bluetooth Controller')


BLACK = (0, 0, 0)
WHITE = (255, 255, 255)
RED = (255, 0, 0)
GRAY = (128, 128, 128)


SHAPE_SIZE = 250
ARROW_SIZE = 200
GAP_SIZE = 50


CENTER = WINDOW_SIZE // 2
CIRCLE_POS = (CENTER, CENTER)
UP_ARROW_POS = (CENTER, CENTER - SHAPE_SIZE // 2 - GAP_SIZE)
DOWN_ARROW_POS = (CENTER, CENTER + SHAPE_SIZE // 2 + GAP_SIZE)
LEFT_ARROW_POS = (CENTER - SHAPE_SIZE // 2 - GAP_SIZE, CENTER)
RIGHT_ARROW_POS = (CENTER + SHAPE_SIZE // 2 + GAP_SIZE, CENTER)


running = True
while running:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            running = False


    keys = pygame.key.get_pressed()


    window.fill(GRAY)


    pygame.draw.circle(window, RED, CIRCLE_POS, SHAPE_SIZE // 2 + 5)


    pygame.draw.polygon(window, BLACK, [(UP_ARROW_POS[0], UP_ARROW_POS[1] - ARROW_SIZE),
                                        (UP_ARROW_POS[0] + ARROW_SIZE // 2, UP_ARROW_POS[1]),
                                        (UP_ARROW_POS[0] - ARROW_SIZE // 2, UP_ARROW_POS[1])])
    pygame.draw.polygon(window, BLACK, [(LEFT_ARROW_POS[0] - ARROW_SIZE, LEFT_ARROW_POS[1]),
                                        (LEFT_ARROW_POS[0], LEFT_ARROW_POS[1] + ARROW_SIZE // 2),
                                        (LEFT_ARROW_POS[0], LEFT_ARROW_POS[1] - ARROW_SIZE // 2)])
    pygame.draw.polygon(window, BLACK, [(DOWN_ARROW_POS[0], DOWN_ARROW_POS[1] + ARROW_SIZE),
                                        (DOWN_ARROW_POS[0] + ARROW_SIZE // 2, DOWN_ARROW_POS[1]),
                                        (DOWN_ARROW_POS[0] - ARROW_SIZE // 2, DOWN_ARROW_POS[1])])
    pygame.draw.polygon(window, BLACK, [(RIGHT_ARROW_POS[0] + ARROW_SIZE, RIGHT_ARROW_POS[1]),
                                        (RIGHT_ARROW_POS[0], RIGHT_ARROW_POS[1] + ARROW_SIZE // 2),
                                        (RIGHT_ARROW_POS[0], RIGHT_ARROW_POS[1] - ARROW_SIZE // 2)])
   
    text_surface = FONT.render('W', True, WHITE)
    window.blit(text_surface, (UP_ARROW_POS[0] - text_surface.get_width() // 2, UP_ARROW_POS[1] - 50))


    text_surface = FONT.render('A', True, WHITE)
    window.blit(text_surface, (LEFT_ARROW_POS[0] - 50, LEFT_ARROW_POS[1] - text_surface.get_height() // 2))


    text_surface = FONT.render('S', True, WHITE)
    window.blit(text_surface, (DOWN_ARROW_POS[0] - text_surface.get_width() // 2, DOWN_ARROW_POS[1] + 50))


    text_surface = FONT.render('D', True, WHITE)
    window.blit(text_surface, (RIGHT_ARROW_POS[0] + 50, RIGHT_ARROW_POS[1] - text_surface.get_height() // 2))


    text_surface = FONT.render('SPACE', True, WHITE)
    window.blit(text_surface, (CIRCLE_POS[0] - text_surface.get_width() // 2, CIRCLE_POS[1] - text_surface.get_height() // 2))


    if keys[pygame.K_w]:
        pygame.draw.polygon(window, WHITE, [(UP_ARROW_POS[0], UP_ARROW_POS[1] - ARROW_SIZE),
                                            (UP_ARROW_POS[0] + ARROW_SIZE // 2, UP_ARROW_POS[1]),
                                            (UP_ARROW_POS[0] - ARROW_SIZE // 2, UP_ARROW_POS[1])], 4)
        ser.write(b'w')


    if keys[pygame.K_a]:
        pygame.draw.polygon(window, WHITE, [(LEFT_ARROW_POS[0] - ARROW_SIZE, LEFT_ARROW_POS[1]),
                                            (LEFT_ARROW_POS[0], LEFT_ARROW_POS[1] + ARROW_SIZE // 2),
                                            (LEFT_ARROW_POS[0], LEFT_ARROW_POS[1] - ARROW_SIZE // 2)], 4)
        ser.write(b'a')


    if keys[pygame.K_s]:
        pygame.draw.polygon(window, WHITE, [(DOWN_ARROW_POS[0], DOWN_ARROW_POS[1] + ARROW_SIZE),
                                            (DOWN_ARROW_POS[0] + ARROW_SIZE // 2, DOWN_ARROW_POS[1]),
                                            (DOWN_ARROW_POS[0] - ARROW_SIZE // 2, DOWN_ARROW_POS[1])], 4)
        ser.write(b's')
       
    if keys[pygame.K_d]:
        pygame.draw.polygon(window, WHITE, [(RIGHT_ARROW_POS[0] + ARROW_SIZE, RIGHT_ARROW_POS[1]),
                                            (RIGHT_ARROW_POS[0], RIGHT_ARROW_POS[1] + ARROW_SIZE // 2),
                                            (RIGHT_ARROW_POS[0], RIGHT_ARROW_POS[1] - ARROW_SIZE // 2)], 4)
        ser.write(b'd')


    if keys[pygame.K_SPACE]:
        pygame.draw.circle(window, WHITE, CIRCLE_POS, SHAPE_SIZE // 2 + 5, 4)
        ser.write(b' ')


    if keys[pygame.K_ESCAPE]:
        pygame.quit()
        ser.close()


    pygame.display.flip()


pygame.quit()

And the arduino code:

#include <SoftwareSerial.h>
#include <QMC5883LCompass.h>

// For Modules
const int bluetoothRX = 7;
const int bluetoothTX = 6;
const int trigger = 5;
const int buzzer = 2;
const int echo = 4;

// For Motor A
const int MotorPinA = 12;
const int MotorSpeedPinA = 3;
const int MotorBrakePinA = 9;

// For Motor B
const int MotorPinB = 13;
const int MotorSpeedPinB = 11;
const int MotorBrakePinB = 8;

const int forward = HIGH;
const int reverse = LOW;

QMC5883LCompass compass;
SoftwareSerial bluetooth(bluetoothRX, bluetoothTX);

int baseHeading = 0;
bool isBaselineSet = false;
bool isMovingForward = false;
bool isMovingBackward = false;

void setup() {
  pinMode(MotorPinA, OUTPUT);
  pinMode(MotorSpeedPinA, OUTPUT);
  pinMode(MotorBrakePinA, OUTPUT);

  pinMode(MotorPinB, OUTPUT);
  pinMode(MotorSpeedPinB, OUTPUT);
  pinMode(MotorBrakePinB, OUTPUT);

  pinMode(trigger, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(buzzer, OUTPUT);

  compass.init();
  bluetooth.begin(9600);
}

void loop() {
  if (bluetooth.available()) {
    char receivedCommand = bluetooth.read();

    if (receivedCommand == 'w') {
      if (!isMovingForward) {
        compass.read();
        baseHeading = compass.getAzimuth();
        isBaselineSet = true;
        isMovingForward = true;
        isMovingBackward = false;
      }
      maintainDirection(forward);
    } else if (receivedCommand == 'a') {
      moveMotor('A', forward, 255);
      moveMotor('B', reverse, 255);
    } else if (receivedCommand == 's') {
      if (!isMovingBackward) {
        compass.read();
        baseHeading = compass.getAzimuth();
        isBaselineSet = true;
        isMovingBackward = true;
        isMovingForward = false;
      }
      maintainDirection(reverse);
    } else if (receivedCommand == 'd') {
      moveMotor('A', reverse, 255);
      moveMotor('B', forward, 255);
    } else if (receivedCommand == ' ') {
      moveMotor('A', reverse, 0);
      moveMotor('B', reverse, 0);
      isMovingForward = false;
      isMovingBackward = false;
      isBaselineSet = false;
    }
  }
}

void moveMotor(char motor, int dir, int speed) {
  int motorPin;
  int motorSpeedPin;

  if (motor == 'A') {
    motorPin = MotorPinA;
    motorSpeedPin = MotorSpeedPinA;
  } else if (motor == 'B') {
    motorPin = MotorPinB;
    motorSpeedPin = MotorSpeedPinB;
  }
  digitalWrite(motorPin, dir);        
  analogWrite(motorSpeedPin, speed);  
}

void maintainDirection(int direction) {
  if (isBaselineSet) {
    compass.read();
    int currentHeading = compass.getAzimuth();
    int error = currentHeading - baseHeading;

    if (error < -180) {
      error += 360;
    } else if (error > 180) {
      error -= 360;
    }

    int speedA = 255;
    int speedB = 255;

    if (error > 0) {
      if (direction == forward) {
        speedA = 255 - map(error, 0, 180, 0, 255);
      } else {
        speedB = 255 - map(error, 0, 180, 0, 255);
      }
    } else if (error < 0) {
      if (direction == forward) {
        speedB = 255 - map(-error, 0, 180, 0, 255);
      } else {
        speedA = 255 - map(-error, 0, 180, 0, 255);
      }
    }

    moveMotor('A', direction, speedA);
    moveMotor('B', direction, speedB);
  }
}

Thiết kế website giá rẻ

Danh mục

Problems with my 2WD Arduino Car using Bluetooth