Arduino互換機+mrubyでラジコンを自動走行してみた

2014/2/20追記
下記はRSTRING_PTR()を誤った使い方をしています。mrb_valueにRSTRING_PTR()マクロを使った場合に取り出した文字列は終端がNULLとは限りませんのでご注意

前回までで、chipKIT Max32上でmrubyプログラムをさくさく開発できるようになりました。何かやることは無いかと考えて、昔遊んでいたラジコンのモータとサーボが制御できることがわかったので、自動走行してみました。

最初に円を描く走行、その次は８の字運転です。

・・・残雪多すぎ＋シャフトが外れて８の字は散々(笑)
どちらもmrubyのコードで制御しています。雪がない時に調整すればもう少しまともに走るはず。

円走行(circle.rb)

Serial.println("loading circle.rb....")

class Circle
	include Arduino

	def initialize
		@servo = Servo.new
		@servo.attach(5)
	end

	def idleloop

	end

	def run
		Serial.println("circle started")
		delay(5000)

		@servo.write(60)
		analogWrite(3, 220)
		delay(10 * 1000)

		analogWrite(3, 128)
		@servo.write(90)
		delay(1000)

	end
end

Circle.new

八の字(eight.rb)

Serial.println("loading eight.rb...")

class Eight
	include Arduino

	def initialize
		@servo = Servo.new
		@servo.attach(5)
	end

	def idleloop
	end

	def angle(val)
		@servo.write(val)
	end

	def speed(val)
　　　　　#モータは3番PINに接続
		analogWrite(3, val)
	end

	#八の字運転
	def draw_8
		
		#straight
		angle 90
		speed  240
		delay 1000

		#turn left
		angle  0
		speed  210
		delay 1500

		#straight
		angle  90
		speed  240
		delay 1000

		#turn right
		angle  180
		speed  210
		delay 1500
	end		

	def run
		Serial.println("eight started")
		delay(5000)

		3.times do
			draw_8
		end

		angle 90
		speed 128

	end
end

Eight.new

angle()は舵角の調整です。0で左いっぱい、90で中央(直進)、180で右いっぱい。
speed()はモータ回転の調整です。最大255で、コーナリング時は少し遅くしています。

digitalWrite()、pinMode()、Servoクラス等のArduinoのAPIのうち、使うものだけmrubyから呼び出せるようにC側で関数を用意してます(mrbgem化したい→mruby-arduinoとしてmrbgem化しました。）。また、今回はスイッチが押されたらrun()がC側から呼ばれます。

かなり長いですが、C側のコードも一応。

#include <Servo.h>


#define CHANGE_HEAP_SIZE(size) __asm__ volatile ("\t.globl _min_heap_size\n\t.equ _min_heap_size, " #size "\n")



#include <mrbconf.h>
#include <mruby.h>

#include <mruby/irep.h>
#include <mruby/string.h>  
#include <mruby/value.h>
#include <mruby/dump.h>
#include <mruby/data.h>
#include <mruby/class.h>



//extern const char initialcode[];

mrb_state *mrb;
char code[4048];
mrb_value sketchObject;

mrb_value mrb_serial_begin(mrb_state *mrb, mrb_value self){
  mrb_int speed = 0;
  int n = mrb_get_args(mrb,"i",&speed);
  Serial.begin(speed);
  return mrb_nil_value();
}

mrb_value mrb_serial_println(mrb_state *mrb, mrb_value self){
  mrb_value s;
  mrb_get_args(mrb,"S", &s);
  Serial.println(RSTRING_PTR(s));
  return mrb_nil_value();
}

void mrb_servo_free(mrb_state *mrb, void *ptr){
  delete (Servo *)ptr;
}

struct mrb_data_type mrb_servo_type = {"Servo", mrb_servo_free};

mrb_value mrb_servo_initialize(mrb_state *mrb, mrb_value self){
  Servo *newServo = new Servo();
  Serial.println("newServo");
  Serial.println((int)newServo);
  DATA_PTR(self) = newServo;  
  DATA_TYPE(self) = &mrb_servo_type;  
  return self;
}

mrb_value mrb_servo_attach(mrb_state *mrb, mrb_value self){
  Servo *servo = (Servo *)mrb_get_datatype(mrb, self, &mrb_servo_type);
  
  mrb_int pin = 0;
  int n = mrb_get_args(mrb, "i", &pin);
  servo->attach(pin);
  return mrb_nil_value();
}

mrb_value mrb_servo_write(mrb_state *mrb, mrb_value self){
  Servo *servo = (Servo *)mrb_get_datatype(mrb, self, &mrb_servo_type);
  mrb_int angle = 0;
  int n = mrb_get_args(mrb, "i", &angle);
  servo->write(angle);
  return mrb_nil_value();
}

mrb_value mrb_arduino_pinMode(mrb_state *mrb, mrb_value self){
  mrb_int pin, mode;
  int n = mrb_get_args(mrb, "ii", &pin, &mode);
  pinMode(pin, mode);
  return mrb_nil_value();
}

mrb_value mrb_arduino_digitalWrite(mrb_state *mrb, mrb_value self){
  mrb_int pin, value;
  int n = mrb_get_args(mrb, "ii", &pin, &value);
  digitalWrite(pin, value);
  return mrb_nil_value();
}

mrb_value mrb_arduino_digitalRead(mrb_state *mrb, mrb_value self){
  mrb_int pin;
  int n = mrb_get_args(mrb, "i", &pin);
  int val = digitalRead(pin);
  return mrb_fixnum_value(val);
}

mrb_value mrb_arduino_analogWrite(mrb_state *mrb, mrb_value self){
  mrb_int pin, value;
  int n = mrb_get_args(mrb, "ii", &pin, &value);
  analogWrite(pin, value);
  return mrb_nil_value();
}

mrb_value mrb_arduino_analogRead(mrb_state *mrb, mrb_value self){
  mrb_int pin;
  int n = mrb_get_args(mrb, "i", &pin);
  int val = analogRead(pin);
  return mrb_fixnum_value(val);
}
  
mrb_value mrb_arduino_delay(mrb_state *mrb, mrb_value self){
  mrb_int ms;
  int n = mrb_get_args(mrb, "i", &ms);
  delay(ms);
  return mrb_nil_value();
}

mrb_value mrb_arduino_millis(mrb_state *mrb, mrb_value self){
  return mrb_fixnum_value(millis());
}

mrb_value mrb_arduino_map(mrb_state *mrb, mrb_value self){
  mrb_int value, fromLow, fromHigh, toLow, toHigh;
  int n = mrb_get_args(mrb, "iiiii", &value, &fromLow, &fromHigh, &toLow, &toHigh);
  mrb_int ret = map(value, fromLow, fromHigh, toLow, toHigh);
  return mrb_fixnum_value(ret);
}


void init_arduino_library(mrb_state *mrb){
  RClass *serialClass = mrb_define_class(mrb, "Serial", mrb->object_class);
  mrb_define_class_method(mrb, serialClass,"begin",mrb_serial_begin, ARGS_REQ(1));
  mrb_define_class_method(mrb, serialClass,"println", mrb_serial_println, ARGS_REQ(1));


  RClass *servoClass = mrb_define_class(mrb, "Servo", mrb->object_class);
  MRB_SET_INSTANCE_TT(servoClass, MRB_TT_DATA);
  mrb_define_method(mrb, servoClass,"initialize", mrb_servo_initialize, ARGS_NONE());
  mrb_define_method(mrb, servoClass,"attach", mrb_servo_attach, ARGS_REQ(1));
  mrb_define_method(mrb, servoClass,"write", mrb_servo_write, ARGS_REQ(1));
  
  RClass *arduinoModule = mrb_define_module(mrb, "Arduino");
  mrb_define_module_function(mrb, arduinoModule, "pinMode", mrb_arduino_pinMode, ARGS_REQ(2));
  mrb_define_module_function(mrb, arduinoModule, "digitalWrite", mrb_arduino_digitalWrite, ARGS_REQ(2));
  mrb_define_module_function(mrb, arduinoModule, "digitalRead", mrb_arduino_digitalRead, ARGS_REQ(1));
  mrb_define_module_function(mrb, arduinoModule, "analogWrite", mrb_arduino_analogWrite, ARGS_REQ(2));
  mrb_define_module_function(mrb, arduinoModule, "analogRead", mrb_arduino_analogRead, ARGS_REQ(1));
  mrb_define_module_function(mrb, arduinoModule, "delay", mrb_arduino_delay, ARGS_REQ(1));
  mrb_define_module_function(mrb, arduinoModule, "millis", mrb_arduino_millis, ARGS_NONE());
  mrb_define_module_function(mrb, arduinoModule, "map", mrb_arduino_map, ARGS_REQ(5));
  mrb_define_const(mrb, arduinoModule, "HIGH", mrb_fixnum_value(1));
  mrb_define_const(mrb, arduinoModule, "LOW", mrb_fixnum_value(0));
  mrb_define_const(mrb, arduinoModule, "INPUT", mrb_fixnum_value(0));
  mrb_define_const(mrb, arduinoModule, "OUTPUT", mrb_fixnum_value(1));
  mrb_define_const(mrb, arduinoModule, "INPUT_PULLUP", mrb_fixnum_value(2));
}  

unsigned short receiveNewCode(){
  unsigned short lengthH = Serial.read();
  while(Serial.available() == 0);
  unsigned short lengthL = Serial.read();
  unsigned short codeLength = (lengthH << 8) | lengthL;
  
  //send first ack("!")
  Serial.write(33);

  unsigned short readedLen = 0;
  while(readedLen < codeLength){
    for (int i = 0 ; i < 100; i++){
      while(Serial.available() == 0);
      code[readedLen] = Serial.read();
      readedLen++;
      if (readedLen == codeLength)
        break;
    }
    Serial.write(35);
  }
  Serial.println("uploaded(from mruby_rc_runner)");   
  
  return readedLen;
}

const int MOTOR_PIN = 3;
const int START_PIN = 2;
const int SERVO_PIN = 5;

const int SPEED_CONTROL_PIN = 0;
const int ANGLE_CONTROL_PIN = 1;

Servo servo;

void setup(){
  
  //the magic code to increase heap size to 100kb
  // see http://www.chipkit.org/forum/viewtopic.php?f=19&t=1565
  CHANGE_HEAP_SIZE(100*1024);
  
  Serial.begin(9600);
  
  pinMode(START_PIN, INPUT);
  pinMode(MOTOR_PIN, OUTPUT);
  analogWrite(MOTOR_PIN, 128);

  
  mrb = NULL;
  
}



void loop(){
  
  if (Serial.available() > 0 ){
    /*  upload mode  */
    
    if (unsigned short codeLen = receiveNewCode()){
      if (mrb) mrb_close(mrb);
      mrb = mrb_open();
      init_arduino_library(mrb);
      FILE *fp = fmemopen(code, codeLen, "rb");  //open buffer as FILE  stream. platform dependent.
      sketchObject = mrb_load_irep_file(mrb, fp);
     
      //mrb_value result = mrb_funcall(mrb, sketchObject, "setup", 0);
      if (mrb->exc){
        Serial.println("exception occured in setup on new code");
      }
    }
    
  }else if (digitalRead(START_PIN)){
    if (!mrb) return;
    mrb_value result = mrb_funcall(mrb, sketchObject, "run", 0);
    if (mrb->exc){
      Serial.println("exception occured in run");
    }
  }else{
    
    if (mrb) {
      mrb_value result = mrb_funcall(mrb, sketchObject, "idleloop", 0);
      if (mrb->exc){
        Serial.println("exception occured in idle loop");
      }
    }
    
    Serial.println("idle loop");
    int speed = analogRead(SPEED_CONTROL_PIN);
    speed /= 4;
    if (speed > 127){
      if (speed > 250){
        speed = 250;
      }
    } else if (speed > 64){
      //backward slow
      speed = 2;
    }else{
      //backward fast
      speed = 1;
    }
    analogWrite(MOTOR_PIN, speed);

    int angle = analogRead(ANGLE_CONTROL_PIN);
    angle = map(angle, 0, 1024, 180, 0);
    
    /*servo.attach(SERVO_PIN);
    servo.write(angle);  
    delay(500);
    servo.detach();
    */
    delay(100);
    
  }
}