driver.rs

//! モーター制御を行うモジュール

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

message.rs

//! メッセージ関連を格納しているモジュール

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,
    },
}

motor.s

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