R-thetaタイプのスカラロボットを動かしてみた

2022/07/31 categories:3D Printer| tags:3D Printer|3D model|SCARA|ROBOT|Seeeduino XIAO|

R-thetaタイプのスカラロボットをSeeeduino XIAOで動かしてみました。

関連資料

接続図

動作イメージ

ソースコード

Seeeduino XIAOのソースコード

#include <Arduino.h>
#include <TimerTC3.h>

const int ledPin =  13;
bool timer_frag = false;
int motor_value[] = {0, 0, 0, 0};
int serial_data = 0;
long period = 2000;

void TimerCnt(){
  timer_frag = true;
}

void setup() {
  pinMode( 0, OUTPUT);
  pinMode( 1, OUTPUT);
  pinMode( 2, OUTPUT);
  pinMode( 3, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(ledPin, OUTPUT);
  Serial.begin(115200);

  digitalWrite(10, LOW);

  TimerTc3.initialize(period);
  TimerTc3.attachInterrupt(TimerCnt);
}

void loop() {

  if( Serial.available() ){
    digitalWrite(ledPin, HIGH);
    serial_data = Serial.read();
      
    switch (serial_data)
    {
    case 0:
      motor_value[0] = 0;
      motor_value[1] = 0;
      motor_value[2] = 0;
      motor_value[3] = 0;
      break;

    case 1:
      motor_value[0] = 0;
      motor_value[1] = 1;
      motor_value[2] = 0;
      motor_value[3] = 0;
      break;

    case 2:
      motor_value[0] = 1;
      motor_value[1] = 1;
      motor_value[2] = 0;
      motor_value[3] = 0;
      break;

    case 3:
      motor_value[0] = 0;
      motor_value[1] = 1;
      motor_value[2] = 0;
      motor_value[3] = 1;
      break;

    case 4:
      motor_value[0] = 1;
      motor_value[1] = 1;
      motor_value[2] = 1;
      motor_value[3] = 1;
      break;

    case 5:
      period += 100;
      TimerTc3.stop();
      TimerTc3.setPeriod(period);
      TimerTc3.start();
      break;

    case 6:
      period -= 100;
      if(period < 1) period = 1;
      TimerTc3.stop();
      TimerTc3.setPeriod(period);
      TimerTc3.start();
      break;

    default:
      break;
    }
    digitalWrite(ledPin, LOW);
  }
  
  if((serial_data>0) & timer_frag){
    for(int i=0; i<4; i++){
      digitalWrite(i, motor_value[i]);
    }
    digitalWrite(0, LOW);
    digitalWrite(1, LOW);
    digitalWrite(2, LOW);
    digitalWrite(3, LOW);
    timer_frag = false;
  }
  
}

PCのソースコード

# -*- coding: utf-8 -*-
import sys
from PyQt6 import QtWidgets, QtCore, QtGui, QtSerialPort
import pygame
import pygame.locals
import time

class Mainwindow(QtWidgets.QMainWindow):
    def __init__(self) -> None:
        super().__init__()

        self.joystick = Joystick(self)

        self.port = QtSerialPort.QSerialPort('COM9')
        self.port.open(QtCore.QIODevice.OpenModeFlag.ReadWrite)

        self.text_edit = QtWidgets.QPlainTextEdit()

        self.m1_cw_button    = Button('M1 CW')
        self.m1_ccw_button   = Button('M1 CCW')
        self.m1m2_cw_button  = Button('M1 M2 CW')
        self.m1m2_ccw_button = Button('M1 M2 CCW')
        
        self.setCentralWidget(QtWidgets.QWidget())
        self.centralWidget().setLayout(QtWidgets.QGridLayout())
        self.centralWidget().layout().addWidget(self.m1_cw_button,    0, 0, 1, 1)
        self.centralWidget().layout().addWidget(self.m1_ccw_button,   1, 0, 1, 1)
        self.centralWidget().layout().addWidget(self.m1m2_cw_button,  0, 1, 1, 1)
        self.centralWidget().layout().addWidget(self.m1m2_ccw_button, 1, 1, 1, 1)
        self.centralWidget().layout().addWidget(self.text_edit,       2, 0, 1, 2)

        self.m1_cw_button.pressed.connect(lambda    : self.send(1))
        self.m1_ccw_button.pressed.connect(lambda   : self.send(2))
        self.m1m2_cw_button.pressed.connect(lambda  : self.send(3))
        self.m1m2_ccw_button.pressed.connect(lambda : self.send(4))
        
        self.m1_cw_button.released.connect(lambda    : self.send(0))
        self.m1_ccw_button.released.connect(lambda   : self.send(0))
        self.m1m2_cw_button.released.connect(lambda  : self.send(0))
        self.m1m2_ccw_button.released.connect(lambda : self.send(0))
        
        self.joystick.B_PRESSED.connect(lambda    : self.send(1))
        self.joystick.A_PRESSED.connect(lambda    : self.send(2))
        self.joystick.L_PRESSED.connect(lambda    : self.send(3))
        self.joystick.R_PRESSED.connect(lambda    : self.send(4))
        self.joystick.DOWN_PRESSED.connect(lambda : self.send(5))
        self.joystick.UP_PRESSED.connect(lambda   : self.send(6))

        self.joystick.A_RELEASED.connect(lambda : self.send(0))
        self.joystick.B_RELEASED.connect(lambda : self.send(0))
        self.joystick.R_RELEASED.connect(lambda : self.send(0))
        self.joystick.L_RELEASED.connect(lambda : self.send(0))

        self.joystick.start()

    def send(self, data):
        self.port.writeData(bytes([data]))
        texts = '{}\n{}'.format(data, self.text_edit.toPlainText()).splitlines()
        self.text_edit.setPlainText('\n'.join(texts[:100]))

    def closeEvent(self, a0: QtGui.QCloseEvent) -> None:
        self.joystick.terminate()
        return super().closeEvent(a0)

class Button(QtWidgets.QPushButton):
    def __init__(self, text):
        super().__init__()
        self.setText(text)
        self.setMinimumSize(60, 60)

class Joystick(QtCore.QThread):
    A_PRESSED = QtCore.pyqtSignal()
    B_PRESSED = QtCore.pyqtSignal()
    X_PRESSED = QtCore.pyqtSignal()
    Y_PRESSED = QtCore.pyqtSignal()
    R_PRESSED = QtCore.pyqtSignal()
    L_PRESSED = QtCore.pyqtSignal()
    UP_PRESSED = QtCore.pyqtSignal()
    DOWN_PRESSED = QtCore.pyqtSignal()
    LEFT_PRESSED = QtCore.pyqtSignal()
    RIGHT_PRESSED = QtCore.pyqtSignal()
    VIEW_PRESSED = QtCore.pyqtSignal()
    MENU_PRESSED = QtCore.pyqtSignal()
    
    A_RELEASED = QtCore.pyqtSignal()
    B_RELEASED = QtCore.pyqtSignal()
    X_RELEASED = QtCore.pyqtSignal()
    Y_RELEASED = QtCore.pyqtSignal()
    R_RELEASED = QtCore.pyqtSignal()
    L_RELEASED = QtCore.pyqtSignal()
    UP_RELEASED = QtCore.pyqtSignal()
    DOWN_RELEASED = QtCore.pyqtSignal()
    LEFT_RELEASED = QtCore.pyqtSignal()
    RIGHT_RELEASED = QtCore.pyqtSignal()
    VIEW_RELEASED = QtCore.pyqtSignal()
    MENU_RELEASED = QtCore.pyqtSignal()

    AXIS_MOVED = QtCore.pyqtSignal(float, float, float, float, float, float)

    def __init__(self, parent):
        super().__init__(parent)
        pygame.init()
        pygame.joystick.init()
        self.joystick = pygame.joystick.Joystick(0)
        self.joystick.init()
        self.hat_prev = 0

    def get_hat(self):
        hat = self.joystick.get_hat(0)
        val  = 0
        val += int(hat[1] ==  1) << 0 # up
        val += int(hat[1] == -1) << 1 # down
        val += int(hat[0] ==  1) << 2 # right
        val += int(hat[0] == -1) << 3 # left
        val, self.hat_prev = val - self.hat_prev, val
        return val

    def run(self) -> None:
        while True:
            for e in pygame.event.get():

                if e.type == pygame.locals.JOYAXISMOTION:
                    self.AXIS_MOVED.emit(
                        self.joystick.get_axis(0), #left horizontal
                        self.joystick.get_axis(1), #left vertical
                        self.joystick.get_axis(2), #right horizontal
                        self.joystick.get_axis(3), #right vertical
                        self.joystick.get_axis(4), #left trigger
                        self.joystick.get_axis(5), #right triggr
                    )

                elif e.type == pygame.locals.JOYHATMOTION:
                    h = self.get_hat()
                    if h > 0:
                        if (h >> 0) & 1: self.UP_PRESSED.emit()
                        if (h >> 1) & 1: self.DOWN_PRESSED.emit()
                        if (h >> 2) & 1: self.RIGHT_PRESSED.emit()
                        if (h >> 3) & 1: self.LEFT_PRESSED.emit()
                    else:
                        if (abs(h) >> 0) & 1: self.UP_RELEASED.emit()
                        if (abs(h) >> 1) & 1: self.DOWN_RELEASED.emit()
                        if (abs(h) >> 2) & 1: self.RIGHT_RELEASED.emit()
                        if (abs(h) >> 3) & 1: self.LEFT_RELEASED.emit()

                elif e.type == pygame.locals.JOYBUTTONDOWN:
                    if e.button == 0: self.A_PRESSED.emit()
                    if e.button == 1: self.B_PRESSED.emit()
                    if e.button == 2: self.X_PRESSED.emit()
                    if e.button == 3: self.Y_PRESSED.emit()
                    if e.button == 4: self.L_PRESSED.emit()
                    if e.button == 5: self.R_PRESSED.emit()
                    if e.button == 6: self.VIEW_PRESSED.emit()
                    if e.button == 7: self.MENU_PRESSED.emit()

                elif e.type == pygame.locals.JOYBUTTONUP:
                    if e.button == 0: self.A_RELEASED.emit()
                    if e.button == 1: self.B_RELEASED.emit()
                    if e.button == 2: self.X_RELEASED.emit()
                    if e.button == 3: self.Y_RELEASED.emit()
                    if e.button == 4: self.L_RELEASED.emit()
                    if e.button == 5: self.R_RELEASED.emit()
                    if e.button == 6: self.VIEW_RELEASED.emit()
                    if e.button == 7: self.MENU_RELEASED.emit()

if __name__ == '__main__':
    app = QtWidgets.QApplication(sys.argv)
    w = Mainwindow()
    w.show()
    app.exec()

Share post

Related Posts

R-thetaタイプのスカラロボットをモデリングしてみた

R-Thetaタイプのスカラロボットをpyqtgraphでシミュレーションしてみた

Seeeduino XIAOで自作キーボード

GT2用タイミングプーリーをモデリングしてみた

3Dプリンタで左手キーボードを作ってみた　1号機

CREE XML-T6のライトをREC-MOUNTSに取り付けるブラケット

3Dプリンタで7インチディスプレイのケースを作ってみた

3DプリンタでXYZロボットを作ってみた

コメント