depthaiのSPIでサーボを操作する
Introduction
この記事は前回の続きです。
前回でdepthaiを通して任意のデータを送信できました。
今回はESP32へのSPIによるデータ送信とESP側での受信処理をやります。
構成として、depthai ---< SPI >--- ESP32 ---< UART >--- PCとしてdepthaiで送ったデータがちゃんと流れていることを確認します。
Host Script (python)
import depthai as dai
def main():
pipeline = dai.Pipeline()
xIn = pipeline.createXLinkIn()
xIn.setStreamName("input_stream")
xSpi = pipeline.createSPIOut()
xSpi.setStreamName("servo")
xSpi.setBusId(0)
xSpi.input.setBlocking(False)
xOut = pipeline.createXLinkOut()
xOut.setStreamName("servo")
xIn.out.link(xOut.input)
xIn.out.link(xSpi.input)
with dai.Device(pipeline) as device:
xIn_queue = device.getInputQueue("input_stream")
xOut_queue = device.getOutputQueue("servo",maxSize=5,blocking=False)
while True:
data_out = dai.RawBuffer()
data_out.data = [0x61,0x62]
xIn_queue.send(data_out)
data_in = xOut_queue.tryGet()
if data_in is not None:
print(data_in.getRaw().data)
if __name__ == "__main__":
main()
Host側は前回とほぼ変化なしですね。
SPIで送信するにはcreateSPIOutでSPIノードを立てます。
BusIdはSPIバスの番号ですが,すけまティックを見る限り"0"しか接続されてないので0固定です。
input.setBlockingはSPI受信のときにブロッキング(読み取れるまで待つ)するか否かのフラグ設定だと思います。これもほとんどFalse固定でいいと思います。
重要なポイントとして、出力したいデータのQueueと同じ名前のstreamNameを設定する必要があります。
この場合、servoの出力と同じデータを吐き出したいのでservoにします。
送信に関して気にすることは無く、xOutの出力をそのままSPIに出力してくれます。
ESP32 Script
void run_demo(){
uint8_t req_success = 0;
dai::SpiApi mySpiApi;
mySpiApi.set_send_spi_impl(&esp32_send_spi);
mySpiApi.set_recv_spi_impl(&esp32_recv_spi);
bool receivedAnyMessage = false;
while(1) {
dai::Message servoDataMsg;
if(mySpiApi.req_message(&servoDataMsg, "servo")){
uart_write_bytes(ECHO_UART_PORT_NUM, servoDataMsg.raw_data.data, servoDataMsg.raw_data.size);
printf("received! m:%s d:%d t:%d\n",servoDataMsg.raw_data.data,servoDataMsg.raw_data.size,servoDataMsg.type==dai::DatatypeEnum::Buffer);
mySpiApi.free_message(&servoDataMsg);
mySpiApi.spi_pop_message("servo");
receivedAnyMessage = true;
}
if(!receivedAnyMessage){
// Delay pooling of messages
usleep(1000);
}
}
}
//Main application
void app_main()
{
init_esp32_spi();
/* Configure parameters of an UART driver,
* communication pins and install the driver */
uart_config_t uart_config = {
.baud_rate = ECHO_UART_BAUD_RATE,
.data_bits = UART_DATA_8_BITS,
.parity = UART_PARITY_DISABLE,
.stop_bits = UART_STOP_BITS_1,
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
.source_clk = UART_SCLK_APB,
};
int intr_alloc_flags = 0;
#if CONFIG_UART_ISR_IN_IRAM
intr_alloc_flags = ESP_INTR_FLAG_IRAM;
#endif
ESP_ERROR_CHECK(uart_driver_install(ECHO_UART_PORT_NUM, BUF_SIZE * 2, 0, 0, NULL, intr_alloc_flags));
ESP_ERROR_CHECK(uart_param_config(ECHO_UART_PORT_NUM, &uart_config));
ESP_ERROR_CHECK(uart_set_pin(ECHO_UART_PORT_NUM, ECHO_TEST_TXD, ECHO_TEST_RXD, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
run_demo();
deinit_esp32_spi();
}
ESP32側に関しては、uart_echo(esp-idf公式)とtwo_streams(luxonis)のサンプルを参考に引用しつつ組んでいます。
app_mainの最初でinit_esp32_spi();を呼んでspiを初期化します。
この関数でセマフォ周りも初期化してくれていて、割り込み処理を(ほぼ)気にせずにSPIを実装できます。
dai::SpiApi mySpiApi;
mySpiApi.set_send_spi_impl(&esp32_send_spi);
mySpiApi.set_recv_spi_impl(&esp32_recv_spi);
その後上記でSpiを初期化します。特に意識せずにサンプルを引っ張ってきています。
mySpiApi.req_message(&servoDataMsg, "servo")
上記でservoストリームのデータをリクエストし、servoDataMsgに格納します。
このservoDataMsgは
struct Message {
Data raw_data;
Metadata raw_meta;
dai::DatatypeEnum type;
};
で、送信データがraw_data、種類がtyoeに格納されます。
また、raw_metaはパット見、大きめのデータ(depthのデータとか)が入るやつでRawBufferを使う際には使わなさそうです。
Nodeなどを自作する際には便利なのかな。
処理が終わったらdataは解放します。
uart部分はほとんどuart_echoのパクリです。
Result
Host側から送信しているデータは"0x61","0x62"でUTF-8で"a","b"にあたります。
受信側のCOMポートを見ると、uartで適切にデータを送れていることがわかります。
Conclusion
一方向ですがdepthaiからサーボを動かす準備ができたので、次はサーボ動作用のプロトコルを組んでみます。
また、送信側がpythonなのが少し癪なのでCppで組めるようにしときます。
サーボ用電源が5Vしかない(定格6-8V)のがちょっと不安ですが、まぁなんとかなると思います。
追記
送信データを次のようにすることでちゃんとサーボ動かせました。
あとはID書き込むノードとか準備して2軸動かしますね。
TxData = [
0x55,
0x55,
0x01,
0x07,
0x01,
0x00FF & (Angle),
0x00FF & (Angle>>8),
0x00FF & (reach_time),
0x00FF & (reach_time>>8)
]
TxData.append(~np.sum(TxData[2:]))
data_out.data = TxData
動作の様子
動いたけど電源が弱い pic.twitter.com/1E1hBK1S3c
— ruru (@r_u__r_u) 2021年11月14日
