ArduinoTank 1号機プログラム - shit_raccoonの電子工作日記

ソースコード

車体側

/*キャタピラーラジコンの完成版*/
#define ROTATION1_PIN 12
#define SPEED1_PIN 3
#define ROTATION2_PIN 13
#define SPEED2_PIN 11
#define RXPIN 2
#define TXPIN 4
 
int left_power, right_power;
void setup(){
  pinMode(ROTATION1_PIN, OUTPUT);
  pinMode(ROTATION2_PIN, OUTPUT);
  pinMode(SPEED1_PIN, OUTPUT);
  pinMode(SPEED2_PIN, OUTPUT);
  digitalWrite(ROTATION1_PIN, HIGH);
  digitalWrite(ROTATION2_PIN, HIGH);
  analogWrite(SPEED1_PIN, 0);
  analogWrite(SPEED2_PIN, 0);
  Serial.begin(9600);
  left_power = 0; right_power = 0;
}
 
void loop(){
  boolean rotation1, rotation2;
  int speed1, speed2, i;
  char c[4];
  if(Serial.available()){
    c[0] = Serial.read();
    if((c[0] & 0b10000000) != 0){
      delay(5);
        c[1] =Serial.read();
        c[2] = Serial.read();
        c[3] = Serial.read();
      left_power = (c[0] & 0b01111111) << 2 | (c[1] & 0b01100000) >> 5;
      right_power = (c[1] & 0b00011111) << 4 | (c[2] & 0b01111000) >> 3;
      left_power -= 255;
      right_power -= 255;
      //以下で回転数の計算 
      //左モーターに関して
    if(left_power >= 0) //正回転の場合
      rotation1 = HIGH;
    else
      rotation1 = LOW;
     //右モーターに関して
    if(right_power >= 0) //正回転の場合
    rotation2 = HIGH;
    else
      rotation2 = LOW;
 
    speed1 = abs(left_power);
    speed2 = abs(right_power);
 
    digitalWrite(ROTATION1_PIN, rotation1);
    digitalWrite(ROTATION2_PIN, rotation2);
    analogWrite(SPEED1_PIN, speed1);
    analogWrite(SPEED2_PIN, speed2);
 
    }
  }
}
 

コントローラー側

//ソースコード: polar_stick_ver2
#include <MsTimer2.h>
#include <math.h>
#define MAX_X 1012
#define MAX_Y 998
#define MIN_X 0
#define MIN_Y 0
#define NEUTRAL_X 414
#define NEUTRAL_Y 422
#define asobi 20
#define STICK_LED 3
 
int timer_count;
int stick_mode;
 
void count_down(){
  if(timer_count > 0)
    timer_count--;
}
 
void button_interrupt(){
  if(timer_count == 0){
    if(stick_mode == 0){
      stick_mode = 1;
      analogWrite(STICK_LED, 255);
    }
    else{
      stick_mode = 0;
      analogWrite(STICK_LED, 0);
    }
    timer_count = 30;
  }
}
void setup(){
  Serial.begin(9600);
  timer_count = 0;
  attachInterrupt(0, button_interrupt, FALLING);
  MsTimer2::set(10, count_down);
  MsTimer2::start();
  attachInterrupt(0, button_interrupt, FALLING);
  stick_mode = 0;
  digitalWrite(STICK_LED, LOW);
}
 
/*
rは中央からの距離, sは角度を表す. rの最大は1である.
sはスティックを前に倒すと0, 左に倒すと-90, 右に倒すと90, 後ろに倒すと180 or -180を返す
*/
void stick_read(double *r, double *s){
    double stick_x, stick_y;
 
 
  //スティック位置の補正
  stick_x = (double)analogRead(0);
  stick_y = (double)analogRead(1);
 
  if(stick_x < NEUTRAL_X){
    stick_x = (stick_x - NEUTRAL_X) / (NEUTRAL_X - MIN_X);
  }
  else{
    stick_x = (stick_x - NEUTRAL_X) / (MAX_X - NEUTRAL_X);
  }
  if(stick_y < NEUTRAL_Y){
    stick_y = (stick_y -NEUTRAL_Y) / (NEUTRAL_Y - MIN_Y);
  }
  else{
    stick_y = (stick_y - NEUTRAL_Y)/ (MAX_Y - NEUTRAL_Y);
  }
/* デバッグ用
  Serial.print(stick_x);
  Serial.print(",");
  Serial.println(stick_y);
  */
  *r = sqrt(pow(stick_x,2) + pow(stick_y, 2));
  if(*r > 1)*r = 1;
  else if(*r < -1)*r = -1;
  *s = atan2(stick_x, stick_y) /3.14 * 180;
}
/*
極座標による入力を二つのモーターの出力に変換する関数.
超信地展開するバージョン.
moterの出力は-255 ~ 255である. -は逆回転を表す
*/
void convert_pwm(double r, double s, int *lp, int *rp){
  double _lp, _rp;
  if(-180 <= s && s < -90){
    _lp = -1;
   _rp = (s + 180) / 45 - 1;
  }
else if(-90 <= s && s < 0){
  _lp = (s + 90) / 45 - 1;
  _rp = 1;
}
else if(0 <= s && s < 90){
   _lp = 1;
   _rp = 1 - s / 45;
  }
else{//90 <= r <= 180の時
   _lp = 1 - (s - 90) / 45;
   _rp = -1;
}
*lp = _lp * r * 255;
*rp = _rp * r * 255;
if(abs(*lp) < 20)*lp = 0;
if(abs(*rp) < 20)*rp = 0;
/*デバグ用
Serial.print(_lp);
Serial.print(",");
Serial.println(_rp);
*/
}
/*
極座標による入力を二つのモーターの出力に変換する関数.
超信地展開しないバージョン.
moterの出力は-255 ~ 255である. -は逆回転を表す
*/
void convert_pwm2(double r, double s, int *lp, int *rp){
  double _lp, _rp;
  if(-180 <= s && s < -90){
    _lp = (s + 180)  / 90 - 1;
   _rp = (s + 180) / 45 - 1;
  }
else 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 * 255;
*rp = _rp * r * 255;
if(abs(*lp) < 20)*lp = 0;
if(abs(*rp) < 20)*rp = 0;
/*デバグ用
Serial.print(_lp);
Serial.print(",");
Serial.println(_rp);
*/
}
 
void make_command(int left_power, int right_power){
  char c0, c1, c2, c3;
  left_power += 255;
  right_power += 255;
  c0 = (left_power & 0b111111100) >> 2;
  c1 = (left_power & 0b000000011) << 5;
  c1 = c1 | ((right_power & 0b111110000)  >> 4);
  c2 = (right_power & 0b000001111) << 3;
  c3 = 0;
  c0 = c0 | 0b10000000;//先頭バイトの先頭ビットを1とする
  c0 = c0 & 0b111111111;
  Serial.print(c0);
  Serial.print(c1);
  Serial.print(c2);
  Serial.print(c3);
}
 
void loop(){
  double r, s;
  int left_power, right_power;//-255 ~ 255でモーター出力を表現
  stick_read(&r, &s);//極座標で値を返す関数
  if(stick_mode == 0)
    convert_pwm2(r,s,&left_power, &right_power);
  else if(stick_mode == 1)
    convert_pwm(r,s,&left_power, &right_power);
  make_command(left_power, right_power);
  /*でバグ用
  Serial.print(left_power);
  Serial.print(",");
  Serial.println(right_power);
  */
  delay(100);
}