シリアルサーボを動かす(3) プログラム編

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


#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);
 }

}

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

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