ライントレースカー1号機のプログラム - shit_raccoonの電子工作日記

プログラムリスト

ライントレースカー側のプログラム

#include <TimerOne.h>
/*
2013/7/23 動作確認済み
2013/7/24 変数の受信部分を変更, D要素を削除, 動作を確認済み
2013/7/25 データ送信部分の内容を小変更, 動作を確認済み
2013/7/30 モーター回転数の変更周期を20msに変更, 動作を確認(振動の収束が遅くなった)
 Arduino Motor SHILED R3
 9のエリアのどこ黒い線の位置を出力するプログラム
 0 ~ 9がエリアで、-1が線が検出できない状態
 */
 
 
//MOTOR SHILED用
#define PWM_A 3
#define BREAK_B 8
#define BREAK_A 9
#define PWM_B 11
#define DIR_A 12
#define DIR_B 13
 
/*ラインセンサーの閾値
 TH1がセンサーの真下に黒線があると考えられる閾値
 */
#define TH1 100
//前進スピードの定義(MAX255)
#define TRACESPEED 120
//比例成分
#define P_VALUE 17
#define I_VALUE 12
#define D_VALUE 12
int TraceSpeed, P_Value, I_Value, D_Value, P_Threshold;
int leftPower, rightPower;
int pos_b;//一回前の線の位置
boolean traceStartFlag, sendDataFlag;
//timer1はデータ送信用
long volatile MainTimer, timer1, timer2;
 
 
void timerInterrupt(){
  MainTimer += 1;  
}
/*
極座標による入力を二つのモーターの出力に変換する関数. 
 超信地展開しないバージョン. 
 moterの出力は-255 ~ 255である. -は逆回転を表す
 r = 0 ~ 255をとる. s = -180 ~ 180をとる. 
 sは前進が0, 左旋回が-90, 
 右旋回が90, 後退が180 or -180である
 */
 
void convert_pwm2(double r, double s, int *lp, int *rp){
  double _lp, _rp;
  if(s > 180 || s < -180)s = 180;
  /*
  if(-180 <= s && s < -90){
    _lp = (s + 180)  / 90 - 1;
    _rp = (s + 180) / 45 - 1;
  }*/
  if(-90 <= s && s < 0){
    _lp = (s + 90) / 90; 
    _rp = 1;
  }
  else if(0 <= s && s < 90){
    _lp = 1;
    _rp = 1 - s / 90;
  }
  /*
  else{//90 <= r <= 180の時
    _lp = 1 - (s - 90) / 45;
    _rp = -1 * (s - 90) / 90;
  }*/
  *lp = _lp * r;
  *rp = _rp * r;
  if(abs(*lp) < 20)*lp = 0;
  if(abs(*rp) < 20)*rp = 0; 
  /*デバグ用
   Serial.print(_lp);
   Serial.print(",");
   Serial.println(_rp);
   */
}
void setup(){
  Serial.begin(9600);
  pinMode(PWM_A, OUTPUT);
  pinMode(PWM_B, OUTPUT);
  pinMode(BREAK_A, OUTPUT);
  pinMode(BREAK_B, OUTPUT);
  pinMode(DIR_A, OUTPUT);
  pinMode(DIR_B, OUTPUT);
  digitalWrite(BREAK_A, LOW);
  digitalWrite(BREAK_B, LOW);
  digitalWrite(DIR_A, HIGH);
  digitalWrite(DIR_B, HIGH);
  analogWrite(PWM_A, 0);
  analogWrite(PWM_B, 0);
  traceStartFlag = false;
  TraceSpeed = TRACESPEED;
  P_Value = P_VALUE; 
  I_Value = I_VALUE;
  D_Value = D_VALUE; 
  P_Threshold = TH1;
  pos_b = 4;
  MainTimer = 0;timer1 = 0;timer2 = 0;
  Timer1.initialize(1000); //(1ms)
  Timer1.attachInterrupt(timerInterrupt); 
}
void loop(){
  //a[i]はセンサーの出力結果を降順にソートしたもの
  //num[i]はセンサー番号を出力の降順にソートしたもの
  int a[4],a_b[4],num[4] = {
    0,1,2,3  }
  , i, j, swap, pos, s;
  byte b[3];
  //シリアルによりコマンドを受け取る
  /*
  PC→ライントレースカー
   コマンド番号, コマンドの内容, 引数
   0. 全停止
   1. トレーススタート, スピード(8bit)
   2. 方向転換
   3. トレーススピード設定, スピード(8bit)
   4. P要素の設定,(8bit)
   5. I要素の設定, (8bit)
   6. D要素の設定,(8bit)
   7. 距離センサーの動作, (10bit)… この値を超えた場合に自動ストップをする
   8. 手動コントロール時のスピード, (8bit)
   9. 手動コントロール時の角度, (9bit) 0 ~ 360度で送信する180度で前進
   10. 設定状態の全送信の要求
   11. センサーデータの送信モードのON/OFF
   12. フォトダイオードの閾値, (10bit)
   */
  if(Serial.available()){
    b[0] = Serial.read();
    if((b[0] & 0b10000000) != 0){
      delay(20);
      b[1] = Serial.read();
      b[2] = Serial.read();
    }
    //0.全停止
    if((b[0] & 0b01111111) == 0b00000000){
      traceStartFlag = false;
    }
    //1.トレーススタート
    else if((b[0] & 0b01111111) == 0b00000001){
      traceStartFlag = true;
      TraceSpeed = (b[1] << 7 | b[2]) & 0b11111111;
    }
      //3.トレーススピード設定
    else if((b[0] & 0b01111111) == 0b00000011){
      TraceSpeed = (b[1] << 7 | b[2]) & 0b11111111;
    }
 
    //4.P要素の設定,(8bit)
    else if((b[0] & 0b01111111) == 0b00000100){
      P_Value = (b[1] << 7 | b[2]) & 0b11111111;
    }
    //5.I要素の設定,(8bit)
    else if((b[0] & 0b01111111) == 0b00000101){
      I_Value = (b[1] << 7 | b[2]) & 0b11111111;
    }
    //6.D要素の設定,(8bit)
    else if((b[0] & 0b01111111) == 0b00000111){
      D_Value = (b[1] << 7 | b[2]) & 0b11111111;
    }
    //10. 設定データの全送信の要求
    else if((b[0] & 0b01111111) == 0b00001010){
      Serial.print("S:");
      Serial.print(TraceSpeed);
      Serial.print(",TH:");
      Serial.print(P_Threshold);
      Serial.print(",P:");
      Serial.print(P_Value);
      Serial.print(";");
    }
    //11. センサーデータの送信のON/OFF
    else if((b[0] & 0b01111111) == 0b00001011){
      if(b[1] != 0 || b[2] != 0)sendDataFlag = true;
      else sendDataFlag = false;
    }
    //12. フォトダイオードの閾値, (10bit)
    else if((b[0] & 0b01111111) == 0b00001100){
      P_Threshold = (b[1] << 7 | b[2]) & 0b1111111111;
    }
 
  }
  //センサーデータの解析を行う
  a[0] = analogRead(0);
  a[1] = analogRead(1);
  a[2] = analogRead(2);
  a[3] = analogRead(3);
  for(i = 0; i < 4; i++)a_b[i] = a[i];
  //値のソートを行う
  for(i = 0; i < 4 -1; i++){
    for(j = 1; j < 4 - i; j++){
      if(a[j] > a[j-1]){
        swap = a[j];
        a[j] = a[j-1]; 
        a[j-1] = swap;
        swap = num[j]; 
        num[j] = num[j-1]; 
        num[j-1] = swap;
      }
    }
  }
  //二つのセンサーの間に黒線があると考えられる場合
  if(a[0] > P_Threshold && a[1] > P_Threshold){
    if(num[0] == 0 && num[1] == 1)pos = 1;
    else if(num[0] == 1 && num[1] == 0)pos = 2;
    else if(num[0] == 1 && num[1] == 2)pos = 4;
    else if(num[0] == 2 && num[1] == 1)pos = 5;
    else if(num[0] == 2 && num[1] == 3)pos = 7;
    else if(num[0] == 3 && num[1] == 2)pos = 8;
    else pos = -1;
  }
 
  //センサーの真下に黒線があると考えられる場合
  else if(a[0] > P_Threshold){
    if(num[0] == 0)pos = 0;
    else if(num[0] == 1)pos = 3;
    else if(num[0] == 2)pos = 6;
    else if(num[0] == 3)pos = 9;
    else pos = -1;
  }
  else{
    pos = -1;
  }
  if(sendDataFlag == true && MainTimer >= timer1 + 200){
   Serial.print("Pos:");
   Serial.print(pos);
   Serial.print(",p0:");
   Serial.print(a_b[0]);
   Serial.print(",p1:");
   Serial.print(a_b[1]);
   Serial.print(",p2:");
   Serial.print(a_b[2]);
   Serial.print(",p3:");
   Serial.print(a_b[3]);
   Serial.print(";");
   timer1 = MainTimer;
  }
  //以下でモーター出力の計算を行う
 
  if(pos >= 0 && MainTimer >= timer2 + 1){
    // + (pos - pos_b) * D_Value;
    s = (pos - 4.5) * P_Value;
    convert_pwm2(TraceSpeed, s , &leftPower, &rightPower);
    pos_b = pos;
    timer2 = MainTimer;
  } 
 
  if(traceStartFlag == true){
    analogWrite(PWM_A, leftPower);
    analogWrite(PWM_B, rightPower);
  }
  else{
    analogWrite(PWM_A, 0);
    analogWrite(PWM_B, 0);
  }
}