Arduinoにイーサネットシールドつけて操縦するロボのソース

横浜ロボ部でちょっと話題になってたので過去の遺産をサルベージ。




操作側は適当にArduinoへUDP通信でデータを投げるだけ
通信フォーマットは
１バイト目　＠固定
２バイト目　モーター１回転方向
３バイト目　モーター１回転速度
４バイト目　モーター２回転方向
５バイト目　モーター２回転速度
になってます。

イーサネットシールドはノイズに弱いようなので、実際にロボ化はしませんでした。

/*
 * Arduinoラジコン
 */
#include <SPI.h>
#include <Ethernet.h>
#include <Udp.h>

/*------------ 定数定義 ------------*/

// モータの制御ピン設定
#define MOTOR1_PWM    (5) // モータ１の出力制御ピン
#define MOTOR1_CTRL1  (6) // モータ１の回転方向制御ピン
#define MOTOR1_CTRL2  (7) // モータ１の回転方向制御ピン
#define MOTOR2_PWM    (10) // モータ２の出力制御ピン
#define MOTOR2_CTRL1  (9) // モータ２の回転方向制御ピン
#define MOTOR2_CTRL2  (8) // モータ２の回転方向制御ピン


// モータの回転方向
#define MOTOR_S       (0) // 停止
#define MOTOR_R       (1) // 右回転
#define MOTOR_L       (2) // 左回転

// 最大タイムアウト回数
#define TIMEOUT_MAX   (2000)


#define PWM_PIN       (0)
#define CTRL1_PIN     (1)
#define CTRL2_PIN     (2)



/*------------ 変数定義 ------------*/

/** イーサネットシールドのMACアドレス */
byte mac[] = { 0x90, 0xA2, 0xDA, 0x00, 0x21, 0x04 };

/** IPアドレス */
byte ip[]  = { 192, 168, 11, 200 };

/** ポート番号 */
unsigned int localPort = 8888;

/** 接続元のIP */
byte remoteIp[4];

/** 接続元のポート番号 */
unsigned int remotePort;

/** 受信データ */
char rcvData[UDP_TX_PACKET_MAX_SIZE];

/** 送信データ */
char sndData[] = "move";

/** タイムアウト回数 */
unsigned int timeout = 0;

unsigned char motor_pin[2][3];

int pwm=0;


/*------------ 関数定義 ------------*/

/**
 * Arduinoの初期設定
 */
void setup() {
  
  // イーサネットで通信開始
  Ethernet.begin(mac,ip);
  Udp.begin(localPort);

  // 動作確認用のシリアル通信
  Serial.begin(9600);
  Serial.println("Wake up");
  
  // モーター制御用のピンを格納
  motor_pin[0][PWM_PIN]   = MOTOR1_PWM;
  motor_pin[0][CTRL1_PIN] = MOTOR1_CTRL1;
  motor_pin[0][CTRL2_PIN] = MOTOR1_CTRL2;
  motor_pin[1][PWM_PIN]   = MOTOR2_PWM;
  motor_pin[1][CTRL1_PIN] = MOTOR2_CTRL1;
  motor_pin[1][CTRL2_PIN] = MOTOR2_CTRL2;
  
  
  // モーター制御用のピンを出力用に設定
  pinMode(MOTOR1_CTRL1, OUTPUT);
  pinMode(MOTOR1_CTRL2, OUTPUT);
  pinMode(MOTOR2_CTRL1, OUTPUT);
  pinMode(MOTOR2_CTRL2, OUTPUT);
  
}



/**
 * モーターの回転制御を行う
 * rotationにMOTOR_S/MOTOR_R/MOTOR_Lを指定する事で回転方向や停止を制御する。
 * 
 * @param[in] ch       モータのチャンネル
 * @param[in] rotation 回転方向(MOTOR_S/MOTOR_R/MOTOR_L)
 * @param[in] power    出力値(0～255)
 */
void ctrlMotor(int ch, int rotation, unsigned char power) {
  
  Serial.print(ch);
  Serial.print("ch ");
  Serial.print(rotation, DEC);
  Serial.print("/");
  Serial.println(power, DEC);
      
  switch(rotation) { 
    default:
    case MOTOR_S:// 停止
      digitalWrite(motor_pin[ch][CTRL1_PIN], LOW);
      digitalWrite(motor_pin[ch][CTRL2_PIN], LOW);
      analogWrite(motor_pin[ch][PWM_PIN], 0);
      break;
      
    case MOTOR_R: // 右回転
      digitalWrite(motor_pin[ch][CTRL1_PIN], LOW);
      digitalWrite(motor_pin[ch][CTRL2_PIN], HIGH);
      analogWrite(motor_pin[ch][PWM_PIN], power);
      break;
      
    case MOTOR_L: // 左回転
      digitalWrite(motor_pin[ch][CTRL1_PIN], HIGH);
      digitalWrite(motor_pin[ch][CTRL2_PIN], LOW);
      analogWrite(motor_pin[ch][PWM_PIN], power);
      break;
  }
}



/**
 * ループ
 * UDP通信で制御用の情報を受信する
 */
void loop() {
  
  unsigned char r1, s1, r2, s2;
  
  // UDPソケットが受信可能になった
  if(Udp.available()) {
    
    Udp.readPacket(rcvData, UDP_TX_PACKET_MAX_SIZE, remoteIp, remotePort);
    
    // マークチェック
    if (rcvData[0] == '@') {
      timeout = 0;
      
      r1 = 0xFF & rcvData[1];
      s1 = 0xFF & rcvData[2];
      r2 = 0xFF & rcvData[3];
      s2 = 0xFF & rcvData[4];
      
      ctrlMotor(0, r1, s1);
      ctrlMotor(1, r2, s2);
    } else {
      Serial.println("Error");
      timeout++;
    }
    
    //Udp.sendPacket( ReplyBuffer, remoteIp, remotePort);
  } else {
    timeout++;
  }
  
  if (timeout >= TIMEOUT_MAX) {
    Serial.println("Time out");
    timeout = 0;
    
    
    ctrlMotor(0, MOTOR_S, 0);
    ctrlMotor(1, MOTOR_S, 0);
    
    
    // テスト用
    /*
    pwm+=10;
    if(pwm > 100) pwm = 0;
    ctrlMotor(0, MOTOR_R, pwm);
    ctrlMotor(1, MOTOR_R, pwm);
    */
  }
  
}