ArduinoTank 1号機プログラム


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

ソースコード

車体側

  1. /*キャタピラーラジコンの完成版*/
  2. #define ROTATION1_PIN 12
  3. #define SPEED1_PIN 3
  4. #define ROTATION2_PIN 13
  5. #define SPEED2_PIN 11
  6. #define RXPIN 2
  7. #define TXPIN 4
  8.  
  9. int left_power, right_power;
  10. void setup(){
  11. pinMode(ROTATION1_PIN, OUTPUT);
  12. pinMode(ROTATION2_PIN, OUTPUT);
  13. pinMode(SPEED1_PIN, OUTPUT);
  14. pinMode(SPEED2_PIN, OUTPUT);
  15. digitalWrite(ROTATION1_PIN, HIGH);
  16. digitalWrite(ROTATION2_PIN, HIGH);
  17. analogWrite(SPEED1_PIN, 0);
  18. analogWrite(SPEED2_PIN, 0);
  19. Serial.begin(9600);
  20. left_power = 0; right_power = 0;
  21. }
  22.  
  23. void loop(){
  24. boolean rotation1, rotation2;
  25. int speed1, speed2, i;
  26. char c[4];
  27. if(Serial.available()){
  28. c[0] = Serial.read();
  29. if((c[0] & 0b10000000) != 0){
  30. delay(5);
  31. c[1] =Serial.read();
  32. c[2] = Serial.read();
  33. c[3] = Serial.read();
  34. left_power = (c[0] & 0b01111111) << 2 | (c[1] & 0b01100000) >> 5;
  35. right_power = (c[1] & 0b00011111) << 4 | (c[2] & 0b01111000) >> 3;
  36. left_power -= 255;
  37. right_power -= 255;
  38. //以下で回転数の計算
  39. //左モーターに関して
  40. if(left_power >= 0) //正回転の場合
  41. rotation1 = HIGH;
  42. else
  43. rotation1 = LOW;
  44. //右モーターに関して
  45. if(right_power >= 0) //正回転の場合
  46. rotation2 = HIGH;
  47. else
  48. rotation2 = LOW;
  49.  
  50. speed1 = abs(left_power);
  51. speed2 = abs(right_power);
  52.  
  53. digitalWrite(ROTATION1_PIN, rotation1);
  54. digitalWrite(ROTATION2_PIN, rotation2);
  55. analogWrite(SPEED1_PIN, speed1);
  56. analogWrite(SPEED2_PIN, speed2);
  57.  
  58. }
  59. }
  60. }
  61.  

コントローラー側

  1. //ソースコード: polar_stick_ver2
  2. #include <MsTimer2.h>
  3. #include <math.h>
  4. #define MAX_X 1012
  5. #define MAX_Y 998
  6. #define MIN_X 0
  7. #define MIN_Y 0
  8. #define NEUTRAL_X 414
  9. #define NEUTRAL_Y 422
  10. #define asobi 20
  11. #define STICK_LED 3
  12.  
  13. int timer_count;
  14. int stick_mode;
  15.  
  16. void count_down(){
  17. if(timer_count > 0)
  18. timer_count--;
  19. }
  20.  
  21. void button_interrupt(){
  22. if(timer_count == 0){
  23. if(stick_mode == 0){
  24. stick_mode = 1;
  25. analogWrite(STICK_LED, 255);
  26. }
  27. else{
  28. stick_mode = 0;
  29. analogWrite(STICK_LED, 0);
  30. }
  31. timer_count = 30;
  32. }
  33. }
  34. void setup(){
  35. Serial.begin(9600);
  36. timer_count = 0;
  37. attachInterrupt(0, button_interrupt, FALLING);
  38. MsTimer2::set(10, count_down);
  39. MsTimer2::start();
  40. attachInterrupt(0, button_interrupt, FALLING);
  41. stick_mode = 0;
  42. digitalWrite(STICK_LED, LOW);
  43. }
  44.  
  45. /*
  46. rは中央からの距離, sは角度を表す. rの最大は1である.
  47. sはスティックを前に倒すと0, 左に倒すと-90, 右に倒すと90, 後ろに倒すと180 or -180を返す
  48. */
  49. void stick_read(double *r, double *s){
  50. double stick_x, stick_y;
  51.  
  52.  
  53. //スティック位置の補正
  54. stick_x = (double)analogRead(0);
  55. stick_y = (double)analogRead(1);
  56.  
  57. if(stick_x < NEUTRAL_X){
  58. stick_x = (stick_x - NEUTRAL_X) / (NEUTRAL_X - MIN_X);
  59. }
  60. else{
  61. stick_x = (stick_x - NEUTRAL_X) / (MAX_X - NEUTRAL_X);
  62. }
  63. if(stick_y < NEUTRAL_Y){
  64. stick_y = (stick_y -NEUTRAL_Y) / (NEUTRAL_Y - MIN_Y);
  65. }
  66. else{
  67. stick_y = (stick_y - NEUTRAL_Y)/ (MAX_Y - NEUTRAL_Y);
  68. }
  69. /* デバッグ用
  70.   Serial.print(stick_x);
  71.   Serial.print(",");
  72.   Serial.println(stick_y);
  73.   */
  74. *r = sqrt(pow(stick_x,2) + pow(stick_y, 2));
  75. if(*r > 1)*r = 1;
  76. else if(*r < -1)*r = -1;
  77. *s = atan2(stick_x, stick_y) /3.14 * 180;
  78. }
  79. /*
  80. 極座標による入力を二つのモーターの出力に変換する関数.
  81. 超信地展開するバージョン.
  82. moterの出力は-255 ~ 255である. -は逆回転を表す
  83. */
  84. void convert_pwm(double r, double s, int *lp, int *rp){
  85. double _lp, _rp;
  86. if(-180 <= s && s < -90){
  87. _lp = -1;
  88. _rp = (s + 180) / 45 - 1;
  89. }
  90. else if(-90 <= s && s < 0){
  91. _lp = (s + 90) / 45 - 1;
  92. _rp = 1;
  93. }
  94. else if(0 <= s && s < 90){
  95. _lp = 1;
  96. _rp = 1 - s / 45;
  97. }
  98. else{//90 <= r <= 180の時
  99. _lp = 1 - (s - 90) / 45;
  100. _rp = -1;
  101. }
  102. *lp = _lp * r * 255;
  103. *rp = _rp * r * 255;
  104. if(abs(*lp) < 20)*lp = 0;
  105. if(abs(*rp) < 20)*rp = 0;
  106. /*デバグ用
  107. Serial.print(_lp);
  108. Serial.print(",");
  109. Serial.println(_rp);
  110. */
  111. }
  112. /*
  113. 極座標による入力を二つのモーターの出力に変換する関数.
  114. 超信地展開しないバージョン.
  115. moterの出力は-255 ~ 255である. -は逆回転を表す
  116. */
  117. void convert_pwm2(double r, double s, int *lp, int *rp){
  118. double _lp, _rp;
  119. if(-180 <= s && s < -90){
  120. _lp = (s + 180) / 90 - 1;
  121. _rp = (s + 180) / 45 - 1;
  122. }
  123. else if(-90 <= s && s < 0){
  124. _lp = (s + 90) / 90;
  125. _rp = 1;
  126. }
  127. else if(0 <= s && s < 90){
  128. _lp = 1;
  129. _rp = 1 - s / 90;
  130. }
  131. else{//90 <= r <= 180の時
  132. _lp = 1 - (s - 90) / 45;
  133. _rp = -1 * (s - 90) / 90;
  134. }
  135. *lp = _lp * r * 255;
  136. *rp = _rp * r * 255;
  137. if(abs(*lp) < 20)*lp = 0;
  138. if(abs(*rp) < 20)*rp = 0;
  139. /*デバグ用
  140. Serial.print(_lp);
  141. Serial.print(",");
  142. Serial.println(_rp);
  143. */
  144. }
  145.  
  146. void make_command(int left_power, int right_power){
  147. char c0, c1, c2, c3;
  148. left_power += 255;
  149. right_power += 255;
  150. c0 = (left_power & 0b111111100) >> 2;
  151. c1 = (left_power & 0b000000011) << 5;
  152. c1 = c1 | ((right_power & 0b111110000) >> 4);
  153. c2 = (right_power & 0b000001111) << 3;
  154. c3 = 0;
  155. c0 = c0 | 0b10000000;//先頭バイトの先頭ビットを1とする
  156. c0 = c0 & 0b111111111;
  157. Serial.print(c0);
  158. Serial.print(c1);
  159. Serial.print(c2);
  160. Serial.print(c3);
  161. }
  162.  
  163. void loop(){
  164. double r, s;
  165. int left_power, right_power;//-255 ~ 255でモーター出力を表現
  166. stick_read(&r, &s);//極座標で値を返す関数
  167. if(stick_mode == 0)
  168. convert_pwm2(r,s,&left_power, &right_power);
  169. else if(stick_mode == 1)
  170. convert_pwm(r,s,&left_power, &right_power);
  171. make_command(left_power, right_power);
  172. /*でバグ用
  173.   Serial.print(left_power);
  174.   Serial.print(",");
  175.   Serial.println(right_power);
  176.   */
  177. delay(100);
  178. }
  179.