前回の回路を使用してサーボの角度を取得しながら角度を変更するプログラムを作成した。


#include <SoftwareSerial.h>

SoftwareSerial mySerial(4, 5); // RX, TX

uint16_t angle = 0;
uint16_t angle_verify = 0;
uint16_t target_angle = 0;

void setup() {
 Serial.begin(115200,SERIAL_8E1); //115200bpsでポートを開く
 mySerial.begin(115200);
 delay(1);
}
void setangle(uint16_t angle){
 byte msg[3] = {0x80, 0x00, 0x00};
 msg[1] = (angle>>7) & 0x7f;
 msg[2] = angle & 0x7f;
 target_angle = angle;
 Serial.write(msg,3);
}
void rec(byte data){
 static int cnt=0;
 static byte msg[3] = {0x80, 0x00, 0x00};

 //1バイト目
 if(data & 0x80){
 cnt = 0;
 }
 else if(cnt<3){
 msg[cnt] = data;
 if(cnt==2){
 uint16_t a = msg[1]<<7 | msg[2];
 if( a!= target_angle || a == angle_verify){
 angle =a;
 mySerial.println(angle);
 }
 angle_verify = a;
 }
 }
 cnt++;
}

void loop() {
 for(int i=0;i<1000;i++){
 setangle(3500);

 while(Serial.available()){
 rec(Serial.read());
 }
 delay(1);
 }
 for(int i=0;i<1000;i++){
 setangle(11500);

 while(Serial.available()){
 rec(Serial.read());
 }
 delay(1);
 }

}

雑ですが動作確認用ですのでご容赦ください。

実際に動作させて、シリアルで返ってきた値をグラフ化してみたらこんなかんじになりました。

servolog

カテゴリー: 組み込み関連

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

関連記事

組み込み関連

IoTサイコンを作る(3) ~SW4STM32編~

sakura.ioを用いたナウでIoTなサイコンを作るシリーズ第3弾で 続きを読む …

組み込み関連

IoTサイコンを作る(2) ~CubeMX編~

前回の続きです。 STM32CubeMXを使って、回路図に従いマイコン 続きを読む …

組み込み関連

IoTサイコンを作る(1) ~回路編~

さくらインターネットからサービス提供されているsakura.ioを用い 続きを読む …