Arduino サーボモータープログラム

2017年12月31日

以下、Arduino nanoのサーボモーター動作チェックに使ったプログラムです。

元記事はこちら

#include <FlexiTimer2.h>
#include <Servo.h>
int   inByte  = 0;
int   LEDstat = 0;
int   value0  = 0;
int   value1  = 0;
int   value2  = 0;
int   value3  = 0;
float A0in    = 0;
int   REF_VAL = 0;
float REF_DEF = 0;
int   Aval    = 0;
int   cont    = 0;
int   cont2   = 0;
int   offsetA = 1023;
int   offsetB = 1023;
int   srvA    = 0;
int   srvB    = 0;
Servo Alpha;
Servo Beta;
//----------------セットアップ-----------------//
void setup() {
  Serial.begin(9600);
  while (!Serial)

  //Pin set
  pinMode(11, OUTPUT);
  pinMode(12, INPUT);
  pinMode(13, OUTPUT);

  //VCC cheack
  analogReference(INTERNAL);
  for(int i=0; i<=5 ; i++){REF_VAL = analogRead(7);}
  REF_DEF = (float)REF_VAL * 11 / 1024;
  analogReference(DEFAULT);

  Serial.println("/-----START Arduino-----/");

  //Status
  Serial.print("VCC : ");
  Serial.print(REF_DEF, 4);
  Serial.println("V");
  Serial.println("LED control");
  Serial.println("0 : OFF");
  Serial.println("1 : ON");

  Serial.println("/-----------------------/");

  //Timer set
  FlexiTimer2::set(500, SerialCNT); // 500ms period
  FlexiTimer2::start();

  Alpha.attach(10);
  Alpha.write(90);
  Beta.attach(9);
  Beta.write(80);
  
}
//----------------メインループ-----------------//
void loop() {
  //シリアル受信処理
  if(Serial.available() > 0) {
    inByte = Serial.read();
    LEDstat = inByte;
  }

  //アナログ入力
  for(int i=0; i<=5 ; i++){value3 = analogRead(3);}
  for(int i=0; i<=5 ; i++){value2 = analogRead(2);}
  for(int i=0; i<=5 ; i++){value1 = analogRead(1);}
  for(int i=0; i<=5 ; i++){value0 = analogRead(0);}
  A0in = REF_DEF * value0 / 1024;
  Aval = value0 / 4;

  if(!digitalRead(12)){
    offsetA = value1 * 2;
    offsetB = value2 * 2;
  }
  
  srvA = map(value1, 0, offsetA, 20, 180);
  srvB = map(value2, 0, offsetB, 80, 180);

  Counter();
  ControlSET();

  //アナログ出力
  if(LEDstat == '5'){
    analogWrite(11, Aval);
  }else{
    analogWrite(11,0);
  }
}
void Counter(){
  if(cont >= value0){
    cont = 0;
  }else{
    cont++;
  }
}
//---------------LEDコントロール---------------//
void ControlSET() {
  if(LEDstat == '1'){
    PORTB |= _BV(5);
  }else
  if(LEDstat == '2'){
    if(!value0){
      PORTB &= ~_BV(5);
    }else
    if(cont < (value0 / 4)){
      PORTB |= _BV(5);
    }else{
      PORTB &= ~_BV(5);
    }
  }else
  if(LEDstat == '3'){
    if(!value0){
      PORTB &= ~_BV(5);
    }else
    if(cont < (value0 / 2)){
      PORTB |= _BV(5);
    }else{
      PORTB &= ~_BV(5);
    }
  }else
  if(LEDstat == '4'){
    Alpha.write(srvA);
    Beta.write(srvB);
  }else
  {
    PORTB &= ~_BV(5);
  }
}
//---------------表示割り込み------------------//
void SerialCNT() {
  long OneSec = millis()/1000;
  
  //表示
  if((OneSec/60/60!=0)){
      Serial.print(OneSec/60/60);
  Serial.print("h");
  }
  if((OneSec/60!=0)){
      Serial.print((OneSec/60) - (OneSec/60/60)*60);
  Serial.print("m");
  }
  Serial.print(OneSec - ((OneSec/60)*60));
  Serial.print("s: ");
  
  Serial.print(value0);
  Serial.print(" : A0= ");
  Serial.print(A0in, 4);
  Serial.print("V ");


  Serial.print("vA= ");
  Serial.print(value1);
  Serial.print(" : ");

  Serial.print("oA= ");
  Serial.print(offsetA);
  Serial.print(" : ");

  Serial.print("sA= ");
  Serial.print(srvA);
  Serial.print(" : ");


  Serial.print("vB= ");
  Serial.print(value2);
  Serial.print(" : ");

  Serial.print("oB= ");
  Serial.print(offsetB);
  Serial.print(" : ");

  Serial.print("sB= ");
  Serial.print(srvB);
  Serial.print(" : ");
  
  Serial.print("Mode key= ");
  Serial.write(LEDstat);
  //Serial.print("   \r");
  Serial.println("   \r");
}