初版

Wheel-Legged Robot

製作品

(GPL-3.0+) GNU General Public License, version 3

# 概要 SPRESENSEを用いて足先がタイヤの4脚ロボット Wheel-Legged Robot を製作しました。 SPRESENSEでのロボット製作は初めてでしたが、多彩なインターフェースやマルチコアCPUを利用し さらには事前にモニター提供いただいたSPRESENSE用のBLE通信ボードや9軸IMUセンサも活用して無事に作り上げることができました。  # 構成 製作した Wheel-Legged Robot の構成は以下のとおりです。  　 　 機体ベースは3Dプリントして組みたてました。  ## 部品 - Spresense メインボード - Spresense 拡張ボード - [BLE1507(NUS firmware)](https://crane-elec.co.jp/products/vol-24/) - [9軸 加速度計・ジャイロ・コンパスセンサ (BMI270・AK09918) Addon ボード](https://nextstep.official.ec/items/66165488) - [シリアルサーボ STS3032](https://www.feetechrc.com/6v-45kg-magnetic-code-360-degree-serial-bus-steering-gear.html) - [シリアルサーボ XL330-M077-T](https://e-shop.robotis.co.jp/product.php?id=416) - [5V 降圧DCDCモジュール](https://amzn.to/3S3IjQ7) - [7.4V 2セル LiPoバッテリ](https://amzn.to/3vGhOZx) # 技術要素 本ロボットに使用した技術やデバイスについて記載します。 ## シリアルサーボ STS3032 ロボットの足駆動にはシリアルサーボ [STS3032](https://www.feetechrc.com/6v-45kg-magnetic-code-360-degree-serial-bus-steering-gear.html) を使用しました。 1足に対して２個のサーボ 合計8個 使用。 　 　 STS3032駆動ライブラリとして以下を使用させていただきました。 Feetech社製シリアルサーボをマイコンのUART IOで直接駆動が可能となります。 - [Feetechのサーボをマイコンから直接動かす](https://qiita.com/Ninagawa123/items/7b79c5f5117dd1470ac9) ここではソフトウェアシリアルでGPIO 2ピンでデータ送信してSTS3032を制御しました。ボーレートは1Mbps。 STS3032からのデータ受信はしていません。 @[twitter](https://twitter.com/H0meMadeGarbage/status/1730920394884817097) ## シリアルサーボ XL330-M077-T 足先のタイヤ駆動としてシリアルサーボ [XL330-M077-T](https://e-shop.robotis.co.jp/product.php?id=416)を使用しました。 設定をDrive Mode：0x08、Operating Mode：PWM control として、連続回転モードで使用します。 無負荷時で300rpm以上の高速で回転します。 @[twitter](https://twitter.com/H0meMadeGarbage/status/1726068833558175946) 　 専用のArduinoライブラリが用意されており、以下の動画やマニュアルを参考に導入しました。 [https://emanual.robotis.com/docs/en/parts/controller/openrb-150/](https://emanual.robotis.com/docs/en/parts/controller/openrb-150/) 　 ここではSPRESENSEのUART2でXL330を制御しました。ボーレートは2Mbps。 自作したサーボ用の[変換基板](https://homemadegarbage.com/sts3032#%E3%82%B3%E3%83%B3%E3%83%88%E3%83%AD%E3%83%BC%E3%83%A9)を介してXL330とのデータ送受信をします。  　 　 通信インターフェースを別にしたことでSPRESENSEでSTS3032とXL330を独立に制御することが可能となりました。 @[twitter](https://twitter.com/H0meMadeGarbage/status/1730929168236646868) ## BLE1507 SPRESENSEメインボードと拡張ボードと合わせてBLE通信ボード [BLE1507 ](https://crane-elec.co.jp/products/vol-24/) もモニター提供いただきました。 このボードを用いてBLEによる遠隔ロボット操作を目指します。 @[twitter](https://twitter.com/H0meMadeGarbage/status/1733304842712748408) 　 BLE1507はSPRESENSEメインボードに載せてUART2でデータ送受信します。 コードやBLE通信方法については以下を参考にしました。 - [BLE1507 用 Nordic UART Service FW について](https://crane-elec.co.jp/wp/wp-content/uploads/2023/06/dcdfe3438217d626aa7018ec377b9b08.pdf) 　 以下の動画ではBLE1507を用いてスマホとのBLE通信で文字データを送受信しています。 スマホアプリ [nRF Toolbox](https://play.google.com/store/apps/details?id=no.nordicsemi.android.nrftoolbox) を使用しています。 @[youtube](https://www.youtube.com/watch?v=yS__bpDHvZM) 　 スマホからの文字データでロボットの動作モードや制御定数の指定を行います。 nRF Toolbox ではマクロで送信データに対応したボタンを設定できるので便利です。  　 BLE1507ボード―SPRESENSE間は UART2での通信となるためシリアルサーボXL330との通信と干渉してしまいます。 そこでXL330のArduinoライブラリをソフトウェアシリアル対応させるのは難しいと考え、ここではBLE1507をソフトウェアシリアル対応するべくボードを加工しました。  BLE1507のシリアル RX, TXピンのピンヘッダを除去して、RXをD16ピンにTXをGNDに配線接続しました。 今回はBLEボードからの送信は行わないためTXはGND接続としました。 この加工によってソフトウェアシリアル(RX：GPIO 16)によるBLE1507通信動作が実現できました。 @[twitter](https://twitter.com/H0meMadeGarbage/status/1740301813394428111) ## 9軸センサ (BMI270・AK09918) Addon ボード モニター提供品4点目として[9軸 加速度計・ジャイロ・コンパスセンサ (BMI270・AK09918) Addon ボード](https://nextstep.official.ec/items/66165488)をいただきました。 6軸のIMUセンサ BMI270と３軸磁気センサAK09918が搭載されたSPRESENSE用センサボードです。  　 BLE通信ボード BLE1507との同梱が可能か心配でしたが無事に干渉なく載りました。  　 本ロボットでは6軸のIMUセンサ BMI270を用いて姿勢の検知を実施します。 BMI270のライブラリ導入方法は以下の記事を参考にしました。 - [SONY Spresense用の6軸センサーBMI270ボードをArduino IDEで動かしてみた](https://qiita.com/kaz19610303/items/2f394e3e35e1b47be3c8) 　 以下では X-Y軸の傾斜センサ動作を楽しみました。 @[twitter](https://twitter.com/H0meMadeGarbage/status/1738163109393842396) 　 BMI270とSPRESENSE間はI2C通信でアドレスは9軸センサボードのジャンパ設定に合わせてBMI2_I2C_SEC_ADDR から BMI2_I2C_PRIM_ADDRに変更しました。 傾斜はIMUから得られた加速度と角速度からカルマンフィルタを通して算出しています。 カルマンフィルタライブラリとして以下を使用しました。 　[https://github.com/TKJElectronics/KalmanFilter](https://github.com/TKJElectronics/KalmanFilter) ## 電源 電源として[7.4V 2セル LiPoバッテリ](https://amzn.to/3vGhOZx)を使用しました。 バッテリを[5V DCDCモジュール](https://amzn.to/3S3IjQ7)で降圧してSPRESENSEに給電しました。 シリアルサーボの電源はそれぞれ以下通りです。 - STS 3032：7.4V (LiPoバッテリ直結) - XL330：5V (降圧DCDC出力) 　 バッテリは機体裏に両面テープ固定しました。 各サーボの配線はユニバーサル基板とコネクタでまとめています。   # 動作 本ロボット Wheel-Legged Robot の動作をまとめます。 ## 走行・変形 タイヤによる走行に加えて足駆動により形状変更が可能です。 @[youtube](https://www.youtube.com/watch?v=WoUrIH7Vq9w) ## 歩行 足駆動による四足歩行動作が可能です。 足は足先の座標を指定し逆運動学(IK)でサーボの角度を算出して制御 @[youtube](https://www.youtube.com/watch?v=JPKv5I45sF0) ## 姿勢保持 IMUセンサによって機体の姿勢を検知して水平保持します。 @[youtube](https://www.youtube.com/watch?v=NJVwzISTeEs) 　 IMUセンサから機体の傾きと角速度を検知して足の高さを制御し機体の水平を保っています。 それぞれの足の高さをセンサのX軸、Y軸の角度と角速度に応じていわゆるPD制御しています。  各軸の角度$\theta$と角速度$\omega$で各足の高さ$h$を制御します。 $$h=h_{-1}+K_p \cdot \theta+K_d \cdot \omega$$ $h_{-1}$は直前に算出した足高さ $K_p$、$K_d$は係数 　 @[youtube](https://www.youtube.com/watch?v=4CAjXlC-6jc) ## 倒立 (番外編) 今回のコードにまとめて盛り込むことはできませんでしたが倒立振子動作も可能です。 @[youtube](https://www.youtube.com/watch?v=R4syw2xcDls) ## 動作まとめ 多種多様な動作をSPRESENSEによって混在させることに成功しました。 @[youtube](https://www.youtube.com/watch?v=IGsYeoD9wNc) # ソースコード コーディングはArduino IDEで実施し以下のように２個のコアを使用しました。 - Mainコア：BLE1507から受信した動作モードやパラメータを受けてサーボ制御。IMUセンサ制御。 - Sub1コア：BLEボード BLE1507 での通信管理。 ## Main ```arduino:Main.ino #ifdef SUBCORE #error "Core selection is wrong!!" #endif #include <MP.h> #include <SoftwareSerial.h> #include <EEPROM.h> #include "BMI270_Arduino.h" #include <Kalman.h> #include <Dynamixel2Arduino.h> #define DXL_SERIAL Serial2 const float DXL_PROTOCOL_VERSION = 2.0; Dynamixel2Arduino dxl(DXL_SERIAL); int sub1 = 1; int sub2 = 2; SoftwareSerial mySerial(-1, 2); // RX,TX int8_t id; int8_t idMode = 10; int8_t idPeriod = 20; int8_t idHeight = 30; int8_t idKp = 40; int8_t idKd = 50; int8_t idX = 60; int8_t idUpHeight = 70; int8_t idStride = 80; int8_t idStop = 90; int8_t idAd = 91; int8_t idBack = 92; int8_t idR = 93; int8_t idL = 94; int8_t idHi = 95; int ret = 0; int Data = 0, Mode = 0; const int Stretch = 1; const int Step = 2; const int Advance = 3; const int Back = 4; const int Imu = 5; const int rover = 11; const int sedan = 12; const int batmobile = 13; // ID 0 1 2 3 4 5 6 7 int offset[] = {0, 55, 0, 68, 0, -13, 28, -81}; //leg length float L1 = 50.0, L2 = 50.0;//unit:mm int period = 120, periodRx, height = 65, heightRx; int upHeight = 10, upHeightRx, stride = 10, strideRx; int x = 0, xRx; float hX = 0.0, hY = 0.0, Kp = 2.0, Kd = 4.0; int KpRx, KdRx; Kalman kalmanX, kalmanY; float kalAngleX, kalAngleDotX, kalAngleY, kalAngleDotY; unsigned long oldTime = 0, loopTime, nowTime; float dt; float theta_X = 0.0, theta_Y = 0.0; float theta_Xdot = 0.0, theta_Ydot = 0.0; BMI270Class BMI270; /* Other functions */ int8_t configure_sensor(struct bmi2_dev *dev); struct bmi2_sens_float sensor_data; /* Sensor configuration */ int8_t configure_sensor(){ int8_t rslt; uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_GYRO }; struct bmi2_sens_config config[2]; /* Configure the type of feature. */ config[0].type = BMI2_ACCEL; /* NOTE: The user can change the following configuration parameters according to their requirement. */ /* Set Output Data Rate */ config[0].cfg.acc.odr = BMI2_ACC_ODR_800HZ; /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; /* The bandwidth parameter is used to configure the number of sensor samples that are averaged * if it is set to 2, then 2^(bandwidth parameter) samples * are averaged, resulting in 4 averaged samples. * Note1 : For more information, refer the datasheet. * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but * this has an adverse effect on the power consumed. */ config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; /* Enable the filter performance mode where averaging of samples * will be done based on above set bandwidth and ODR. * There are two modes * 0 -> Ultra low power mode * 1 -> High performance mode(Default) * For more info refer datasheet. */ config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; /* Configure the type of feature. */ config[1].type = BMI2_GYRO; /* The user can change the following configuration parameters according to their requirement. */ /* Set Output Data Rate */ config[1].cfg.gyr.odr = BMI2_GYR_ODR_800HZ; /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ config[1].cfg.gyr.range = BMI2_GYR_RANGE_250; /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; /* Enable/Disable the noise performance mode for precision yaw rate sensing * There are two modes * 0 -> Ultra low power mode(Default) * 1 -> High performance mode */ config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; /* Enable/Disable the filter performance mode where averaging of samples * will be done based on above set bandwidth and ODR. * There are two modes * 0 -> Ultra low power mode * 1 -> High performance mode(Default) */ config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; /* Set the accel configurations. */ rslt = BMI270.set_sensor_config(config, 2); if (rslt != BMI2_OK) return rslt; rslt = BMI270.sensor_enable(sens_list, 2); if (rslt != BMI2_OK) return rslt; return rslt; } //加速度センサから傾きデータ取得 [deg] void getAccGyro() { BMI270.bmi2_get_sensor_float(&sensor_data); theta_X = atan2(sensor_data.acc.y, sensor_data.acc.z) * 57.29578f; theta_Y = atan2(-sensor_data.acc.x, sensor_data.acc.z) * 57.29578f; theta_Xdot = sensor_data.gyr.x; theta_Ydot = sensor_data.gyr.y; } void setup() { Serial.begin(115200, SERIAL_8N1); mySerial.begin(1000000); Serial2.begin(2000000, SERIAL_8N1); MP.begin(sub1); MP.RecvTimeout(MP_RECV_POLLING); //Polling 受信待ちなし delay(100); int8_t rslt = BMI270.begin(BMI270_I2C,BMI2_I2C_PRIM_ADDR); rslt = configure_sensor(); delay(200); for(int i = 0; i < 10; i++){ getAccGyro(); delay(2); } kalmanX.setAngle(theta_X); kalmanY.setAngle(theta_Y); EEPROM.get(idPeriod, periodRx); if(periodRx != 0) period = periodRx; EEPROM.get(idHeight, heightRx); if(heightRx != 0) height = heightRx; EEPROM.get(idX, xRx); if(xRx != 0) x = xRx; EEPROM.get(idUpHeight, upHeightRx); if(upHeightRx != 0) upHeight = upHeightRx; EEPROM.get(idStride, strideRx); if(strideRx != 0) stride = strideRx; EEPROM.get(idKp, KpRx); if(KpRx != 0) Kp = KpRx / 10.0; EEPROM.get(idKd, KdRx); if(KdRx != 0) Kd = KdRx / 10.0; fRIK(x, height); fLIK(x, height); rLIK(x, height); rRIK(x, height); // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate. dxl.begin(2000000); // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); // Turn off torque when configuring items in EEPROM area dxl.torqueOff(0); dxl.torqueOff(1); dxl.torqueOff(2); dxl.torqueOff(3); dxl.setOperatingMode(0, OP_PWM); dxl.setOperatingMode(1, OP_PWM); dxl.setOperatingMode(2, OP_PWM); dxl.setOperatingMode(3, OP_PWM); dxl.torqueOn(0); dxl.torqueOn(1); dxl.torqueOn(2); dxl.torqueOn(3); dxl.setGoalPWM(0, 0.0, UNIT_PERCENT); dxl.setGoalPWM(1, 0.0, UNIT_PERCENT); dxl.setGoalPWM(2, 0.0, UNIT_PERCENT); dxl.setGoalPWM(3, 0.0, UNIT_PERCENT); } void loop() { ret = MP.Recv(&id, &Data, sub1); if (ret == idMode) { if (Data == idAd) { //前進 dxl.setGoalPWM(0, 20.0, UNIT_PERCENT); dxl.setGoalPWM(1, -20.0, UNIT_PERCENT); dxl.setGoalPWM(2, 20.0, UNIT_PERCENT); dxl.setGoalPWM(3, -20.0, UNIT_PERCENT); }else if(Data == idBack){ //後進 dxl.setGoalPWM(0, -20.0, UNIT_PERCENT); dxl.setGoalPWM(1, 20.0, UNIT_PERCENT); dxl.setGoalPWM(2, -20.0, UNIT_PERCENT); dxl.setGoalPWM(3, 20.0, UNIT_PERCENT); }else if(Data == idR){ //右 dxl.setGoalPWM(0, -20.0, UNIT_PERCENT); dxl.setGoalPWM(1, -20.0, UNIT_PERCENT); dxl.setGoalPWM(2, -20.0, UNIT_PERCENT); dxl.setGoalPWM(3, -20.0, UNIT_PERCENT); }else if(Data == idL){ //左 dxl.setGoalPWM(0, 20.0, UNIT_PERCENT); dxl.setGoalPWM(1, 20.0, UNIT_PERCENT); dxl.setGoalPWM(2, 20.0, UNIT_PERCENT); dxl.setGoalPWM(3, 20.0, UNIT_PERCENT); }else if(Data == idHi){ //高速前進 dxl.setGoalPWM(0, 80.0, UNIT_PERCENT); dxl.setGoalPWM(1, -80.0, UNIT_PERCENT); dxl.setGoalPWM(2, 80.0, UNIT_PERCENT); dxl.setGoalPWM(3, -80.0, UNIT_PERCENT); }else if(Data == idStop){ //停止 dxl.setGoalPWM(0, 0.0, UNIT_PERCENT); dxl.setGoalPWM(1, 0.0, UNIT_PERCENT); dxl.setGoalPWM(2, 0.0, UNIT_PERCENT); dxl.setGoalPWM(3, 0.0, UNIT_PERCENT); }else{ Mode = Data; } }else if (ret == idPeriod) { period = Data; EEPROM.put(idPeriod, period); }else if (ret == idHeight) { height = Data; EEPROM.put(idHeight, height); }else if (ret == idKp) { Kp = Data / 10.0; EEPROM.put(idKp, Data); }else if (ret == idKd) { Kd = Data / 10.0; EEPROM.put(idKd, Data); }else if (ret == idX) { x = Data; EEPROM.put(idX, Data); }else if (ret == idUpHeight) { upHeight = Data; EEPROM.put(idUpHeight, Data); }else if (ret == idStride) { stride = Data; EEPROM.put(idStride, Data); } if(Mode == Stretch){ //屈伸 float tim,tt; unsigned long time_mSt= millis(); tim=0; while(tim<period * 8){ tim=millis()-time_mSt; tt=float(tim * 2 * PI / (period * 8)); fRIK(x, height + upHeight * sin(tt)); fLIK(x, height + upHeight * sin(tt)); rLIK(x, height + upHeight * sin(tt)); rRIK(x, height + upHeight * sin(tt)); } }else if(Mode == Step){ //足踏み float tim,tt; unsigned long time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(x, height - upHeight * sin(tt)); rLIK(x, height - upHeight * sin(tt)); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(x, height - upHeight * cos(tt)); rLIK(x, height - upHeight * cos(tt)); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); rRIK(x, height - upHeight * sin(tt)); fLIK(x, height - upHeight * sin(tt)); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); rRIK(x, height - upHeight * cos(tt)); fLIK(x, height - upHeight * cos(tt)); } }else if(Mode == Advance){ //前進 float tim,tt; unsigned long time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(x - stride * cos(tt), height - upHeight * sin(tt)); fLIK(x + stride * cos(tt), height); rLIK(x + stride * cos(tt), height - upHeight * sin(tt)); rRIK(x - stride * cos(tt), height); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(x + stride * sin(tt), height - upHeight * cos(tt)); fLIK(x - stride * sin(tt), height); rLIK(x - stride * sin(tt), height - upHeight * cos(tt)); rRIK(x + stride * sin(tt), height); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(x + stride * cos(tt), height); fLIK(x - stride * cos(tt), height - upHeight * sin(tt)); rLIK(x - stride * cos(tt), height); rRIK(x + stride * cos(tt), height - upHeight * sin(tt)); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(x - stride * sin(tt), height); fLIK(x + stride * sin(tt), height - upHeight * cos(tt)); rLIK(x + stride * sin(tt), height); rRIK(x - stride * sin(tt), height - upHeight * cos(tt)); } }else if(Mode == Back){ //後進 float tim,tt; unsigned long time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(x + stride * cos(tt), height - upHeight * sin(tt)); fLIK(x - stride * cos(tt), height); rLIK(x - stride * cos(tt), height - upHeight * sin(tt)); rRIK(x + stride * cos(tt), height); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(x - stride * sin(tt), height - upHeight * cos(tt)); fLIK(x + stride * sin(tt), height); rLIK(x + stride * sin(tt), height - upHeight * cos(tt)); rRIK(x - stride * sin(tt), height); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(x - stride * cos(tt), height); fLIK(x + stride * cos(tt), height - upHeight * sin(tt)); rLIK(x + stride * cos(tt), height); rRIK(x - stride * cos(tt), height - upHeight * sin(tt)); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(x + stride * sin(tt), height); fLIK(x - stride * sin(tt), height - upHeight * cos(tt)); rLIK(x - stride * sin(tt), height); rRIK(x + stride * sin(tt), height - upHeight * cos(tt)); } }else if(Mode == Imu){ nowTime = micros(); loopTime = nowTime - oldTime; oldTime = nowTime; dt = (float)loopTime / 1000000.0; //sec getAccGyro(); //カルマンフィルタ 姿勢 傾き kalAngleX = kalmanX.getAngle(theta_X, theta_Xdot, dt); kalAngleY = kalmanY.getAngle(theta_Y, theta_Ydot, dt); //カルマンフィルタ 姿勢 角速度 kalAngleDotX = kalmanX.getRate(); kalAngleDotY = kalmanY.getRate(); Serial.print(Kp); Serial.print(","); Serial.println(Kd); hX = hX + Kp * kalAngleY / 90.0 + Kd * kalAngleDotY / 100.0; hY = hY + Kp * kalAngleX / 90.0 + Kd * kalAngleDotX / 100.0; float hs = hX - hY; hs = constrain(hs, -30, 30); float hd = hX + hY; hd = constrain(hd, -30, 30); fRIK(x, height - hd); fLIK(x, height - hs); rRIK(x, height + hs); rLIK(x, height + hd); }else if(Mode == rover){ fRIK(0, 50); fLIK(0, 50); rLIK(0, 50); rRIK(0, 50); }else if(Mode == sedan){ fRIK(80, 50); fLIK(80, 50); rLIK(70, 60); rRIK(70, 60); }else if(Mode == batmobile){ fRIK(98, 15); fLIK(98, 15); rLIK(30, 15); rRIK(30, 15); }else if(Mode == 0){ //初期姿勢 fRIK(x, height); fLIK(x, height); rLIK(x, height); rRIK(x, height); dxl.setGoalPWM(0, 0.0, UNIT_PERCENT); dxl.setGoalPWM(1, 0.0, UNIT_PERCENT); dxl.setGoalPWM(2, 0.0, UNIT_PERCENT); dxl.setGoalPWM(3, 0.0, UNIT_PERCENT); Serial.print(x); Serial.print(","); Serial.println(height); } } //参考 https://qiita.com/Ninagawa123/items/7b79c5f5117dd1470ac9 void sts_moveToPos(byte id, int position) { // コマンドパケットを作成 byte message[13]; message[0] = 0xFF; // ヘッダ message[1] = 0xFF; // ヘッダ message[2] = id; // サーボID message[3] = 9; // パケットデータ長 message[4] = 3; // コマンド（3は書き込み命令） message[5] = 42; // レジスタ先頭番号 message[6] = position & 0xFF; // 位置情報バイト下位 message[7] = (position >> 8) & 0xFF; // 位置情報バイト上位 message[8] = 0x00; // 時間情報バイト下位 message[9] = 0x00; // 時間情報バイト上位 message[10] = 0x00; // 速度情報バイト下位 message[11] = 0x00; // 速度情報バイト上位 // チェックサムの計算 byte checksum = 0; for (int i = 2; i < 12; i++) { checksum += message[i]; } message[12] = ~checksum; // チェックサム // コマンドパケットを送信 for (int i = 0; i < 13; i++) { mySerial.write(message[i]); } } void servo_write(int ch, float ang){ int sig = 2047 + offset[ch] + int((2048.0 / 180.0) * ang); sts_moveToPos(ch, sig); delayMicroseconds(500); } //------------------------------------------- //IK //------------------------------------------- void fRIK(float x, float z){ float phi,ld, th1, th2; ld = sqrt(x*x + z*z); phi = atan2(-x, z); th1 = phi - acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld)); th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)); servo_write(4, th1 * 180.0 / PI); servo_write(6, th2 * 180.0 / PI); } void fLIK(float x, float z){ float phi,ld, th1, th2; ld = sqrt(x*x + z*z); phi = atan2(-x, z); th1 = phi - acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld)); th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)); servo_write(5, -(th1 * 180.0 / PI)); servo_write(7, -(th2 * 180.0 / PI)); } void rRIK(float x, float z){ float phi,ld, th1, th2; ld = sqrt(x*x + z*z); phi = atan2(-x, z); th1 = phi - acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld)); th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)); servo_write(1, -(th1 * 180.0 / PI)); servo_write(3, -(th2 * 180.0 / PI)); } void rLIK(float x, float z){ float phi,ld, th1, th2; ld = sqrt(x*x + z*z); phi = atan2(-x, z); th1 = phi - acos((L1*L1 - L2*L2 +ld*ld)/(2*L1*ld)); th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)); servo_write(0, th1 * 180.0 / PI); servo_write(2, th2 * 180.0 / PI); } ``` ## Sub1 ```arduino:Sub1.ino #if (SUBCORE != 1) #error "Core selection is wrong!!" #endif #include <MP.h> #include <SoftwareSerial.h> SoftwareSerial mySerial(16, -1); // RX,TX int8_t idMode = 10; int8_t idPeriod = 20; int8_t idHeight = 30; int8_t idKp = 40; int8_t idKd = 50; int8_t idX = 60; int8_t idUpHeight = 70; int8_t idStride = 80; int8_t idStop = 90; int8_t idAd = 91; int8_t idBack = 92; int8_t idR = 93; int8_t idL = 94; int8_t idHi = 95; String rcv; int Data = 0; void setup() { mySerial.begin(115200); delay(300); MP.begin(); } void loop() { while(mySerial.available() != 0){ rcv = mySerial.readStringUntil('\n'); if (rcv.indexOf('t') != -1) { Data = rcv.substring(1).toInt(); MP.Send(idPeriod, Data); }else if (rcv.indexOf('h') != -1) { Data = rcv.substring(1).toInt(); MP.Send(idHeight, Data); }else if (rcv.indexOf('p') != -1) { Data = rcv.substring(1).toInt(); MP.Send(idKp, Data); }else if (rcv.indexOf('d') != -1) { Data = rcv.substring(1).toInt(); MP.Send(idKd, Data); }else if (rcv.indexOf('x') != -1) { Data = rcv.substring(1).toInt(); MP.Send(idX, Data); }else if (rcv.indexOf('u') != -1) { Data = rcv.substring(1).toInt(); MP.Send(idUpHeight, Data); }else if (rcv.indexOf('s') != -1) { Data = rcv.substring(1).toInt(); MP.Send(idStride, Data); }else{ Data = rcv.toInt(); MP.Send(idMode, Data); } } } ``` # おわりに ここではSPRESENSEによる足先がタイヤの4脚ロボット Wheel-Legged Robot の製作を楽しみました。 これまでSPRESENSEを用いてロボットを製作しようという発想はなかったのですが、本コンテストに"ロボット部門"が設立されていることから真正面からSPRESENSEによるロボット製作に挑みました。 SPRESENSE自身の多様なインターフェースとマルチコアCPUによって多機能化が容易であり、専用のセンサや通信ボードのよる拡張で見事にロボットを作りきることができました。 前回のコンテスト(2022)ではSPRESENSEの省エネ機能を活用して[バッテリレス監視システム](https://elchika.com/article/0b74da45-51a3-47ab-9e95-3142f88ef6cd/)を製作しました。 今回はSPRESENSEの豊富な物理資産を活用して多機能ロボットを製作し、SPRESENSEのハードウェアとしての有用性を強く実感することができました。 次回はいよいよSPRESENSEのAI機能について学習し活用方法を考えられればと夢が広がっております。 このような気づきの機会を与えてくださったコンテストやサイトの運営様・関係者様そして素晴らしいデバイスを提供くださった企業様に感謝申し上げます。