サーボ制御その2

サーボのプログラムを更新しました

サーボ(といくかアクチュエイター)の負荷を下げるために稼働時間を短く(速く動かす)しています

 

#include <Servo.h>

unsigned long mytime = 0;
unsigned long hittime;
unsigned long duration;
boolean sensorBool = false;//センサ計測制御用
boolean servoUp = false;//servo上下のBool
boolean serialFilter = true;//servo上下のBool
int input_value;//A1入力値
Servo myservo;//サーボ設定
int kaisu = 0;  // 角度のカウンタ max10で使うのは8がいい
int power;//サーボ動作速度
unsigned long startTime;//タイマー用
int servoTime;

void stageReset() {
  myservo.write(0);//最大斜度にする(リセット)
  delay(5000);//最大斜度になるのを待つ
  
  //約4度傾けて水平にする
  servoUp = true;
  power = 180;
  startTime = millis();
  servoTime = 3200;

  kaisu = 4;//4度にセット
}


void setup() {
  Serial.begin(9600);
  myservo.attach(9, 1000, 2000);
  mytime = millis();
  stageReset();//ステージ水平にする
}

void mysend() {
  Serial.println(duration);//シリアルに計測時間送る
}


void loop() {
  //回転間隔計測
  input_value = analogRead(1);
  if (sensorBool) {
    if (input_value < 130) {//3.3V is under 130 is read sw ON Long cable version
      sensorBool = false;
      hittime = millis();
      duration = hittime - mytime;
      mysend();
      mytime = hittime;
    }
  } else if (!sensorBool) {
    if (input_value > 300) {
      sensorBool = true;
    }
  }

  //サーボ動作
  if (servoUp) {
    if (millis() > startTime + servoTime) {
      servoUp = false;
      serialFilter = true;
    }
    myservo.writeMicroseconds(map(power, 0, 255, 1500, 2000));
  } else if (!servoUp) {
    
    myservo.writeMicroseconds(map(0, 0, 255, 1500, 2000));
    Serial.flush();
  }
}

void serialEvent() {
  if (Serial.available()) {
      if(serialFilter){
    serialFilter = false;
    byte var = Serial.read();
    int myvar = var - 0x30;//16進数でくるので16進数のゼロひく
   //int myvar = int(var);
   // switch (var) {
    //  case 1://stage UP
    if(myvar > 8){
      myvar = 8;
    }else if (myvar < 0){
      myvar = 0;
    }
      if(kaisu > -1 && kaisu < 9){  //ex2 var0
        int sabun = myvar - kaisu;
//Serial.println(sabun);
        servoUp = true;
        if(sabun > 0){
        power = 240;///up
        }else if (sabun < 0){
          power = -220;//down
        }
        startTime = millis();
        servoTime = 350 * abs(sabun);
       // if(kaisu > -1 && kaisu < 9){
          if(myvar < 0){
            kaisu = 0;
          }else if (myvar >8){
            kaisu = 8;
          }else{
          kaisu = myvar;
          }
        //  Serial.println(kaisu);
        //}
        //kaisu--;
      }
    //    break;
     // case 2://stage Down
      //else if(kaisu < 9){
       // power = -180;
       // servoUp = true;
       // servoTime = 700;
       // startTime = millis();
       // kaisu++;
      //}
       // break;
    }
  }
  }

 

なお,これに対応するUnity側のスクリプトは

using UnityEngine;
using System.Collections;
using System;//use for time
using UnityEngine.UI;//use for UI
using System.Threading;

public class serialSender2 : MonoBehaviour {
	public SerialHandler serialHandler;
	float myangle;
	int kakudoDummy;
	//Thread mythread;//use thread
	public Text myuitext;

	void Start () {
		Invoke("startLoop", 10f);//10秒後にコルーチン開始
	}

	void Update () {

	}
		
public void startLoop(){
	StartCoroutine(angleSend());
}


IEnumerator angleSend() {
while (true) {
			myangle  = this.transform.localEulerAngles.x;
			if (myangle < 360f && myangle > 180f) {
				myangle = 360f - myangle;
			}else{
				myangle = (myangle * -1f) ;
			}
			myangle = Mathf.Round(myangle) + 4;//まるめた角度
			if (myangle > 8) {
				myangle = 8;
			} else if (myangle < 0) {
				myangle = 0;
			}
			string _myangle  = myangle.ToString();
			serialHandler.Write (_myangle);//send Angle
			//ui display

			myuitext.text = myangle.ToString ();
//			Debug.Log(_myangle);
		yield return new WaitForSeconds(5f);
	}
}
		

}

です

角度を5秒ごとにサンプリングし,0〜8の数値(角度の近似値)に変化させシリアルで送信しています