ライントレースカー1号機のプログラム


※上記の広告は60日以上更新のないWIKIに表示されています。更新することで広告が下部へ移動します。

プログラムリスト

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

  1. #include <TimerOne.h>
  2. /*
  3. 2013/7/23 動作確認済み
  4. 2013/7/24 変数の受信部分を変更, D要素を削除, 動作を確認済み
  5. 2013/7/25 データ送信部分の内容を小変更, 動作を確認済み
  6. 2013/7/30 モーター回転数の変更周期を20msに変更, 動作を確認(振動の収束が遅くなった)
  7.  Arduino Motor SHILED R3
  8.  9のエリアのどこ黒い線の位置を出力するプログラム
  9.  0 ~ 9がエリアで、-1が線が検出できない状態
  10.  */
  11.  
  12.  
  13. //MOTOR SHILED用
  14. #define PWM_A 3
  15. #define BREAK_B 8
  16. #define BREAK_A 9
  17. #define PWM_B 11
  18. #define DIR_A 12
  19. #define DIR_B 13
  20.  
  21. /*ラインセンサーの閾値
  22.  TH1がセンサーの真下に黒線があると考えられる閾値
  23.  */
  24. #define TH1 100
  25. //前進スピードの定義(MAX255)
  26. #define TRACESPEED 120
  27. //比例成分
  28. #define P_VALUE 17
  29. #define I_VALUE 12
  30. #define D_VALUE 12
  31. int TraceSpeed, P_Value, I_Value, D_Value, P_Threshold;
  32. int leftPower, rightPower;
  33. int pos_b;//一回前の線の位置
  34. boolean traceStartFlag, sendDataFlag;
  35. //timer1はデータ送信用
  36. long volatile MainTimer, timer1, timer2;
  37.  
  38.  
  39. void timerInterrupt(){
  40. MainTimer += 1;
  41. }
  42. /*
  43. 極座標による入力を二つのモーターの出力に変換する関数.
  44.  超信地展開しないバージョン.
  45.  moterの出力は-255 ~ 255である. -は逆回転を表す
  46.  r = 0 ~ 255をとる. s = -180 ~ 180をとる.
  47.  sは前進が0, 左旋回が-90,
  48.  右旋回が90, 後退が180 or -180である
  49.  */
  50.  
  51. void convert_pwm2(double r, double s, int *lp, int *rp){
  52. double _lp, _rp;
  53. if(s > 180 || s < -180)s = 180;
  54. /*
  55.   if(-180 <= s && s < -90){
  56.   _lp = (s + 180) / 90 - 1;
  57.   _rp = (s + 180) / 45 - 1;
  58.   }*/
  59. if(-90 <= s && s < 0){
  60. _lp = (s + 90) / 90;
  61. _rp = 1;
  62. }
  63. else if(0 <= s && s < 90){
  64. _lp = 1;
  65. _rp = 1 - s / 90;
  66. }
  67. /*
  68.   else{//90 <= r <= 180の時
  69.   _lp = 1 - (s - 90) / 45;
  70.   _rp = -1 * (s - 90) / 90;
  71.   }*/
  72. *lp = _lp * r;
  73. *rp = _rp * r;
  74. if(abs(*lp) < 20)*lp = 0;
  75. if(abs(*rp) < 20)*rp = 0;
  76. /*デバグ用
  77.   Serial.print(_lp);
  78.   Serial.print(",");
  79.   Serial.println(_rp);
  80.   */
  81. }
  82. void setup(){
  83. Serial.begin(9600);
  84. pinMode(PWM_A, OUTPUT);
  85. pinMode(PWM_B, OUTPUT);
  86. pinMode(BREAK_A, OUTPUT);
  87. pinMode(BREAK_B, OUTPUT);
  88. pinMode(DIR_A, OUTPUT);
  89. pinMode(DIR_B, OUTPUT);
  90. digitalWrite(BREAK_A, LOW);
  91. digitalWrite(BREAK_B, LOW);
  92. digitalWrite(DIR_A, HIGH);
  93. digitalWrite(DIR_B, HIGH);
  94. analogWrite(PWM_A, 0);
  95. analogWrite(PWM_B, 0);
  96. traceStartFlag = false;
  97. TraceSpeed = TRACESPEED;
  98. P_Value = P_VALUE;
  99. I_Value = I_VALUE;
  100. D_Value = D_VALUE;
  101. P_Threshold = TH1;
  102. pos_b = 4;
  103. MainTimer = 0;timer1 = 0;timer2 = 0;
  104. Timer1.initialize(1000); //(1ms)
  105. Timer1.attachInterrupt(timerInterrupt);
  106. }
  107. void loop(){
  108. //a[i]はセンサーの出力結果を降順にソートしたもの
  109. //num[i]はセンサー番号を出力の降順にソートしたもの
  110. int a[4],a_b[4],num[4] = {
  111. 0,1,2,3 }
  112. , i, j, swap, pos, s;
  113. byte b[3];
  114. //シリアルによりコマンドを受け取る
  115. /*
  116.   PC→ライントレースカー
  117.   コマンド番号, コマンドの内容, 引数
  118.   0. 全停止
  119.   1. トレーススタート, スピード(8bit)
  120.   2. 方向転換
  121.   3. トレーススピード設定, スピード(8bit)
  122.   4. P要素の設定,(8bit)
  123.   5. I要素の設定, (8bit)
  124.   6. D要素の設定,(8bit)
  125.   7. 距離センサーの動作, (10bit)… この値を超えた場合に自動ストップをする
  126.   8. 手動コントロール時のスピード, (8bit)
  127.   9. 手動コントロール時の角度, (9bit) 0 ~ 360度で送信する180度で前進
  128.   10. 設定状態の全送信の要求
  129.   11. センサーデータの送信モードのON/OFF
  130.   12. フォトダイオードの閾値, (10bit)
  131.   */
  132. if(Serial.available()){
  133. b[0] = Serial.read();
  134. if((b[0] & 0b10000000) != 0){
  135. delay(20);
  136. b[1] = Serial.read();
  137. b[2] = Serial.read();
  138. }
  139. //0.全停止
  140. if((b[0] & 0b01111111) == 0b00000000){
  141. traceStartFlag = false;
  142. }
  143. //1.トレーススタート
  144. else if((b[0] & 0b01111111) == 0b00000001){
  145. traceStartFlag = true;
  146. TraceSpeed = (b[1] << 7 | b[2]) & 0b11111111;
  147. }
  148. //3.トレーススピード設定
  149. else if((b[0] & 0b01111111) == 0b00000011){
  150. TraceSpeed = (b[1] << 7 | b[2]) & 0b11111111;
  151. }
  152.  
  153. //4.P要素の設定,(8bit)
  154. else if((b[0] & 0b01111111) == 0b00000100){
  155. P_Value = (b[1] << 7 | b[2]) & 0b11111111;
  156. }
  157. //5.I要素の設定,(8bit)
  158. else if((b[0] & 0b01111111) == 0b00000101){
  159. I_Value = (b[1] << 7 | b[2]) & 0b11111111;
  160. }
  161. //6.D要素の設定,(8bit)
  162. else if((b[0] & 0b01111111) == 0b00000111){
  163. D_Value = (b[1] << 7 | b[2]) & 0b11111111;
  164. }
  165. //10. 設定データの全送信の要求
  166. else if((b[0] & 0b01111111) == 0b00001010){
  167. Serial.print("S:");
  168. Serial.print(TraceSpeed);
  169. Serial.print(",TH:");
  170. Serial.print(P_Threshold);
  171. Serial.print(",P:");
  172. Serial.print(P_Value);
  173. Serial.print(";");
  174. }
  175. //11. センサーデータの送信のON/OFF
  176. else if((b[0] & 0b01111111) == 0b00001011){
  177. if(b[1] != 0 || b[2] != 0)sendDataFlag = true;
  178. else sendDataFlag = false;
  179. }
  180. //12. フォトダイオードの閾値, (10bit)
  181. else if((b[0] & 0b01111111) == 0b00001100){
  182. P_Threshold = (b[1] << 7 | b[2]) & 0b1111111111;
  183. }
  184.  
  185. }
  186. //センサーデータの解析を行う
  187. a[0] = analogRead(0);
  188. a[1] = analogRead(1);
  189. a[2] = analogRead(2);
  190. a[3] = analogRead(3);
  191. for(i = 0; i < 4; i++)a_b[i] = a[i];
  192. //値のソートを行う
  193. for(i = 0; i < 4 -1; i++){
  194. for(j = 1; j < 4 - i; j++){
  195. if(a[j] > a[j-1]){
  196. swap = a[j];
  197. a[j] = a[j-1];
  198. a[j-1] = swap;
  199. swap = num[j];
  200. num[j] = num[j-1];
  201. num[j-1] = swap;
  202. }
  203. }
  204. }
  205. //二つのセンサーの間に黒線があると考えられる場合
  206. if(a[0] > P_Threshold && a[1] > P_Threshold){
  207. if(num[0] == 0 && num[1] == 1)pos = 1;
  208. else if(num[0] == 1 && num[1] == 0)pos = 2;
  209. else if(num[0] == 1 && num[1] == 2)pos = 4;
  210. else if(num[0] == 2 && num[1] == 1)pos = 5;
  211. else if(num[0] == 2 && num[1] == 3)pos = 7;
  212. else if(num[0] == 3 && num[1] == 2)pos = 8;
  213. else pos = -1;
  214. }
  215.  
  216. //センサーの真下に黒線があると考えられる場合
  217. else if(a[0] > P_Threshold){
  218. if(num[0] == 0)pos = 0;
  219. else if(num[0] == 1)pos = 3;
  220. else if(num[0] == 2)pos = 6;
  221. else if(num[0] == 3)pos = 9;
  222. else pos = -1;
  223. }
  224. else{
  225. pos = -1;
  226. }
  227. if(sendDataFlag == true && MainTimer >= timer1 + 200){
  228. Serial.print("Pos:");
  229. Serial.print(pos);
  230. Serial.print(",p0:");
  231. Serial.print(a_b[0]);
  232. Serial.print(",p1:");
  233. Serial.print(a_b[1]);
  234. Serial.print(",p2:");
  235. Serial.print(a_b[2]);
  236. Serial.print(",p3:");
  237. Serial.print(a_b[3]);
  238. Serial.print(";");
  239. timer1 = MainTimer;
  240. }
  241. //以下でモーター出力の計算を行う
  242.  
  243. if(pos >= 0 && MainTimer >= timer2 + 1){
  244. // + (pos - pos_b) * D_Value;
  245. s = (pos - 4.5) * P_Value;
  246. convert_pwm2(TraceSpeed, s , &leftPower, &rightPower);
  247. pos_b = pos;
  248. timer2 = MainTimer;
  249. }
  250.  
  251. if(traceStartFlag == true){
  252. analogWrite(PWM_A, leftPower);
  253. analogWrite(PWM_B, rightPower);
  254. }
  255. else{
  256. analogWrite(PWM_A, 0);
  257. analogWrite(PWM_B, 0);
  258. }
  259. }
  260.