Skip to content

Instantly share code, notes, and snippets.

@kb10uy
Created October 5, 2021 10:22
Show Gist options
  • Save kb10uy/0cf4cf9f9f0626ef4632cc713b19b321 to your computer and use it in GitHub Desktop.
Save kb10uy/0cf4cf9f9f0626ef4632cc713b19b321 to your computer and use it in GitHub Desktop.
//! モーター制御を行うモジュール
use crate::message::Operation;
use serialport::prelude::*;
use std::{error::Error, time::Duration};
use tokio::sync::mpsc::Receiver;
/// モータードライバーとの通信を担当する
pub struct MotorDriver {
uart: Box<dyn SerialPort>,
receiver: Receiver<Operation>,
}
impl MotorDriver {
/// 新しいインスタンス
pub fn new(port: &str, receiver: Receiver<Operation>) -> Result<MotorDriver, Box<dyn Error>> {
let uart_config = SerialPortSettings {
data_bits: DataBits::Eight,
baud_rate: 4800,
flow_control: FlowControl::None,
stop_bits: StopBits::One,
parity: Parity::None,
timeout: Duration::from_millis(100),
};
let uart = serialport::open_with_settings(port, &uart_config)?;
Ok(MotorDriver { uart, receiver })
}
/// ドライバーとの通信動作を開始する。
pub async fn run(&mut self) -> Result<(), Box<dyn Error + Send + Sync>> {
loop {
match self.receiver.recv().await {
Some(msg) => {
self.send_operation(msg)?;
}
None => {
break;
}
}
}
Ok(())
}
fn send_operation(&mut self, op: Operation) -> Result<(), Box<dyn Error + Send + Sync>> {
match op {
Operation::Direction { left, right } => {
let cmd = [b'D', left.to_data_char(), right.to_data_char()];
self.uart.write(&cmd)?;
self.check_response()?;
}
Operation::Speed { left, right } => {
let cmd = [b'S', (left * 255.0) as u8, (right * 255.0) as u8];
self.uart.write(&cmd)?;
self.check_response()?;
}
}
Ok(())
}
fn check_response(&mut self) -> Result<(), Box<dyn Error + Send + Sync>> {
let mut buffer = [0];
self.uart.read_exact(&mut buffer)?;
if buffer[0] == b'.' {
Ok(())
} else {
Err("Motor driver reported error".to_string().into())
}
}
}
//! メッセージ関連を格納しているモジュール
use serde::Deserialize;
/// 静的型付けされたメッセージ
#[derive(Debug, Deserialize)]
#[serde(tag = "type", content = "data")]
pub enum ClientMessage {
#[serde(rename = "recog")]
Recognition { position: f32, max_speed: f32 },
#[serde(rename = "param")]
SetParameter { p: f32, i: f32, d: f32 },
}
/// モーター回転方向
#[derive(Debug)]
pub enum Direction {
/// 正転
Forward,
/// 逆転
Backward,
/// ストップ
Stop,
/// ブレーキ
Brake,
}
impl Direction {
/// ドライバーに送る文字(に対応する u8)に変換する
pub fn to_data_char(&self) -> u8 {
match self {
Direction::Forward => b'^',
Direction::Backward => b'v',
Direction::Stop => b'-',
Direction::Brake => b'!',
}
}
}
/// モーター制御の内容
#[derive(Debug)]
pub enum Operation {
/// 方向
Direction {
/// 左タイヤの回転方向
left: Direction,
/// 右タイヤの回転方向
right: Direction,
},
/// 速さ
Speed {
/// 左タイヤの速さ
left: f32,
/// 右タイヤの速さ
right: f32,
},
}
UBRRH = 0x02
UCSRC = 0x03
UBRRL = 0x09
UCSRB = 0x0a
UCSRA = 0x0b
UDR = 0x0c
DDRD = 0x11
PORTD = 0x12
DDRB = 0x17
PORTB = 0x18
OCR1BL = 0x28
OCR1AL = 0x2A
TCCR1B = 0x2e
TCCR1A = 0x2f
TCCR0A = 0x30
TCNT0 = 0x32
TCCR0B = 0x33
MCUCR = 0x35
OCR0A = 0x36
TIMSK = 0x39
SREG = 0x3f
; .def rSREGs = r0 ; SREG 退避(保存)
; .def rZERO = r16 ; 0x00(保存)
; .def rMAX = r17 ; 0x00(保存)
; .def rIMED = r18 ; 即値格納(破壊)
; .def rARG1 = r19 ; 引数
# 割り込みベクターテーブル
.org 0x0000
.ivt:
rjmp .init ; Reset
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .timer0_cmp_a ; Timer0 Compare A
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
rjmp .isr_trap
# 割り込みトラップ
.org 0x0030
.isr_trap:
rjmp .isr_trap
# スタートアップ
.org 0x0040
.init:
cli
; ポート初期化
clr r16
ser r17
ldi r18, 0x18
out DDRB, r18 ; PORTB 3, 4 output, other input
ldi r18, 0xe7
out PORTB, r18 ; pull-up, zero
out DDRD, r17 ; PORTD output
; Timer0 設定
ldi r18, 0x02 ; Timer0 is CTC mode
out TCCR0A, r18
ldi r18, 0x20 ; 32 count
out OCR0A, r18
in r18, TIMSK ; Unmask Timer0 Compare A
sbr r18, 0x01
out TIMSK, r18
; PWM Timer1 設定
ldi r18, 0xa1 ; Mode 5 (Fast PWM 8bit)
out TCCR1A, r18
ldi r18, 0x09 ; Fosc
out TCCR1B, r18
; UART 設定
ldi r18, 0x0c ; 1MHz / 4800bps / 16 - 1
out UBRRH, r16
out UBRRL, r18
ldi r18, 0x18 ; TX/RX enabled
out UCSRB, r18
ldi r18, 0x06 ; 8bit, No parity, 1 stop bit
out UCSRC, r18
; UART Ready
ldi r19, '#
rcall .uart_send_byte
sei
# メインルーチン
.main:
; コマンドバイト受信
rcall .uart_receive_byte
1: cpi r19, 'D ; Direction
brne 2f
rcall .command_direction
rjmp 4f
2: cpi r19, 'S ; Speed
brne 3f
rcall .command_speed
rjmp 4f
3: ldi r19, '! ; Invalid
rcall .uart_send_byte
4: rcall .set_flash
rjmp .main
# Timer0 比較割り込み
.timer0_cmp_a:
in r0, SREG
mov r1, r18
ldi r18, 0x00 ; Stop clock source
out TCCR0B, r18
in r18, PORTD
cbr r18, 0x40
out PORTD, r18
mov r18, r1
out SREG, r0
reti
# ['D', dL, dR] : 回転方向を制御
# dL, dR : 回転方向を表す文字
# - '^' 正回転
# - 'v' 逆回転
# - '-' ストップ
# - '!' ブレーキ
.command_direction:
; 2byte 受信
rcall .uart_receive_byte
mov r20, r19
rcall .uart_receive_byte
mov r21, r19
1: mov r19, r20
rcall .check_direction_type
cpi r19, '!
breq 3f
mov r24, r19
lsl r24
lsl r24
2: mov r19, r21
rcall .check_direction_type
cpi r19, '!
breq 3f
add r24, r19
lsl r24
lsl r24
; この時点で 00LLRR00
out PORTD, r24
rjmp 4f
; 失敗レスポンス
3: ldi r19, '!
rcall .uart_send_byte
ret
; 成功レスポンス
4: ldi r19, '.
rcall .uart_send_byte
ret
# ['S', sL, sR] : 速度を制御
# sL, sR : 速度 0x00〜0xff
.command_speed:
; 2byte 受信
rcall .uart_receive_byte
mov r20, r19
rcall .uart_receive_byte
mov r21, r19
; PWM 更新
out OCR1AL, r20
out OCR1BL, r21
; 受信レスポンス
ldi r19, '.
rcall .uart_send_byte
ret
# r19 の ^ v - ! を判定し、正しければ対応する値を、
# 不正なら ! を返す (to r19)
.check_direction_type:
1: cpi r19, '^
brne 2f
ldi r19, 0x02 ; CW [IN1:IN2] = 0b10
ret
2: cpi r19, 'v
brne 3f
ldi r19, 0x01 ; CCW 0b01
ret
3: cpi r19, '-
brne 4f
ldi r19, 0x00 ; Stop
ret
4: cpi r19, '!
brne 5f
ldi r19, 0x03 ; Brake
ret
5: ldi r19, '!
ret
.set_flash:
out TCNT0, r16 ; Reset Timer0 Counter
ldi r18, 0x05 ; Prescaler Fosc/1024
out TCCR0B, r18
in r18, PORTD ; Set the flasher LED bit
sbr r18, 0x40
out PORTD, r18
ret
# r19 に 1byte 受信する
.uart_receive_byte:
1: sbis UCSRA, 7 ; Check RXC flag
rjmp 1b
in r19, UDR
ret
# r19 を 1byte 送信する
.uart_send_byte:
1: sbis UCSRA, 5
rjmp 1b
out UDR, r19
ret
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment