×

らずらいと姫の挑戦日記(第23回)~加速度センサーでモータを動かす~

2016-09-20

10月4日から開催するCEATECに出展するデモの開発の3回めです。
前回でモーターの準備が出来たので、加速度センサーの値を無線で送信して、それに連動してモーターを動かすデモを完成させました。

1

手を振ったら値を検出して、無線で送信するプログラム

#define LED 26						// pin number of Blue LED
#define SUBGHZ_CH		36			// channel number (frequency)
#define SUBGHZ_PANID	0xabcd		// panid
#define HOST_ADDRESS	0x1003		// distination address

void setup() {
  byte rc;

	SubGHz.init();					// 920MHz無線の初期化
	Serial.begin(115200);			//  シリアルインタフェースの初期化

	Wire.begin();

	rc = kxg03.init(KXG03_DEVICE_ADDRESS_4E);
	rc = bm1422.init(0);
	rc = bm1383.init(0);
}

float mag,x,y,z;
float shake_force;

unsigned char send_data[128];

void loop() {
	SUBGHZ_MSG msg;

	byte rc;
	int i;

	static float val[11];
	float x2,y2,z2;

//	rc = kxg03.get_val(&val[0]);
//	bm1422.get_val(&val[6]);
//	bm1383.get_val(&val[9]);
/*
	x2 = val[3]*val[3];
	y2 = val[4]*val[4];
	z2 = val[5]*val[5];

	mag = sqrt(x2+y2+z2);
	x = atan(val[3]/sqrt(y2+z2))*180/PI;
	y = atan(val[4]/sqrt(z2+x2))*180/PI;
	z = atan(val[5]/sqrt(x2+y2))*180/PI;
	*/

	rc = kxg03.get_val(&val[0]);
	x2 = val[3]*val[3];
	y2 = val[4]*val[4];
	z2 = val[5]*val[5];

	mag = sqrt(x2+y2+z2);

	shake_force = mag-1;
	shake_force = fabs(shake_force);

	x = atan(val[3]/sqrt(y2+z2))*180/PI;
	y = atan(val[4]/sqrt(z2+x2))*180/PI;
	z = atan(val[5]/sqrt(x2+y2))*180/PI;

    Serial.print("STX,");
    Serial.print_double((double)mag, 2);		// Acc(x)
    Serial.print(",");
    Serial.print_double((double)x, 2);		// Acc(x)
    Serial.print(",");
    Serial.print_double((double)y, 2);		// Acc(y)
    Serial.print(",");
    Serial.print_double((double)z, 2);		// Acc(z)
    Serial.print(",");
    Serial.print_double((double)shake_force, 2);		// Acc(z)
    Serial.print(",");
/*    Serial.print_double((double)val[0], 2);		// Gyro(x)
    Serial.print(",");
    Serial.print_double((double)val[1], 2);		// Gyro(y)
    Serial.print(",");
    Serial.print_double((double)val[2], 2);		// Gyro(z)
    Serial.print(",");
    Serial.print_double((double)val[3], 2);		// Acc(x)
    Serial.print(",");
    Serial.print_double((double)val[4], 2);		// Acc(y)
    Serial.print(",");
    Serial.print_double((double)val[5], 2);		// Acc(z)
    Serial.print(",");
    Serial.print_double((double)val[6], 2);		// Mag(x)
    Serial.print(",");
    Serial.print_double((double)val[7], 2);		// Mag(y)
    Serial.print(",");
    Serial.print_double((double)val[8], 2);		// Mag(z)
    Serial.print(",");
    Serial.print_double((double)val[10], 4);		// Pressure

*/
    Serial.println(",ETX");

    if(shake_force > 0.5)
    {
    	Print.init(send_data,sizeof(send_data));
    	Print.l(shake_force*20,DEC);
	 	SubGHz.begin(SUBGHZ_CH, SUBGHZ_PANID,  SUBGHZ_100KBPS, SUBGHZ_PWR_20MW);		// start Sub-GHz
		msg=SubGHz.send(SUBGHZ_PANID, HOST_ADDRESS, &send_data, Print.len(),NULL);// send data
	 	SubGHz.close();
    }	// Sub-GHz module sets into power down mode.
 	sleep(100);  

}

LazuriteIDEサンプルプログラムの「Axisio-serial」と「Welcome_SubGHz」を合体させます。

  • 無線の設定(CH・PANID・HOST_ADDRESS)を受信機側とあわせます。
  • 52~59行目が加速度センサーの値から色々と計算しているところになります。magは加速度の大きさを表しています。そこから、重力分の1Gをひいて振ったときの力 shake_forceを出しました。今回はプラスマイナス関係なく力だけを取りたかったので、fabsで絶対値に直しています。
  • 95行目あたりで振った値が0.5より大きかったら、値×20のデータを無線で送信するように設定しました。この値でモーターを回しています。

手をあげたら値を検出して、無線で送信するプログラム

振ったら回るプログラムとほぼ同じですが、95行目~98行目が違う部分です。


if(y > 30)
    {
    	Print.init(send_data,sizeof(send_data));
    	Print.l(y,DEC);

手の角度が30度位でモーターが回るようにしたいので、yの値が30以上だったら値を送信するように設定しました。

値を受信したらモーターが回るプログラム

LazuriteIDEサンプルプログラムの「Print_SubGHz」をベースに作っています。

#define SUBGHZ_CH	36
#define SUBGHZ_PANID	0xABCD
uint8_t rx_data[256];
uint32_t last_recv_time = 0;
SUBGHZ_STATUS rx;							// structure for getting rx status
#define BLUE_LED	26

void setup(void)
{
	SUBGHZ_MSG msg;
	long myAddress;

	Serial.begin(115200);

	msg = SubGHz.init();
	if(msg != SUBGHZ_OK)
	{
		SubGHz.msgOut(msg);
		while(1){ }
	}

	myAddress = SubGHz.getMyAddress();
	Serial.print("myAddress1 = ");
	Serial.println_long(myAddress,HEX);
	msg = SubGHz.begin(SUBGHZ_CH, SUBGHZ_PANID,  SUBGHZ_100KBPS, SUBGHZ_PWR_20MW);
	if(msg != SUBGHZ_OK)
	{
		SubGHz.msgOut(msg);
		while(1){ }
	}
	msg = SubGHz.rxEnable(NULL);
	if(msg != SUBGHZ_OK)
	{
		SubGHz.msgOut(msg);
		while(1){ }
	}

	pinMode(BLUE_LED,OUTPUT);
	digitalWrite(BLUE_LED,HIGH);

	Serial.println("TIME	HEADER	SEQ	PANID	RX_ADDR	TX_ADDR	RSSI	PAYLOAD");
	Serial.println("-----------------------------------------------------------------------------------------------------------------");

	digitalWrite(17,HIGH);
	digitalWrite(2,LOW);

	pinMode(17,OUTPUT);
	pinMode(2,OUTPUT);

	hhb.init(0,1023);			// h-bridge 100hz
	hhb.init(1,1023);			// h-bridge 100hz
	hhb.init(2,1023);			// h-bridge 100hz
	hhb.init(3,1023);			// h-bridge 100hz

	hhb.attach(0,9,3);
	hhb.attach(1,16,8);
	hhb.attach(2,4,5);
	hhb.attach(3,6,7);

	hhb.write(0,0);
	hhb.write(1,0);
	hhb.write(2,0);
	hhb.write(3,0);

	hhb.start(0);
	hhb.start(1);
	hhb.start(2);
	hhb.start(3);

	hhb.update();

	return;
}

void loop(void)
{
	SUBGHZ_MAC_PARAM mac;
	short rx_len;
	short index=0;
	uint16_t data16;
	int8_t motor_ch;
	int i;
	static uint32_t last_update_time[4]={0,0,0,0};
	bool motor_update = false;

	rx_len = SubGHz.readData(rx_data,sizeof(rx_data));

	if(rx_len>0)	{
		digitalWrite(BLUE_LED, LOW);
		SubGHz.getStatus(NULL,&rx);							// get status of rx
		SubGHz.decMac(&mac,rx_data,rx_len);

		Serial.print_long(millis(),DEC);
		Serial.print("\t");

		Serial.print_long(mac.mac_header.header,HEX);
		Serial.print("\t");

		Serial.print_long(mac.seq_num,HEX);
		Serial.print("\t");

		Serial.print_long(mac.rx_panid,HEX);
		Serial.print("\t");

		data16 = *((uint16_t *)mac.rx_addr);
		Serial.print_long(data16,HEX);
		Serial.print("\t");

		data16 = *((uint16_t *)mac.tx_addr);
		Serial.print_long(data16,HEX);
		Serial.print("\t");

		Serial.print_long(rx.rssi,DEC);
		Serial.print("\t");

		Serial.print(mac.payload);
		// print ln
		Serial.println("");
		digitalWrite(BLUE_LED, HIGH);

		data16 = *((uint16_t *)mac.tx_addr);

		switch(data16)
		{
		case 0x3FFA://shake
			motor_ch=0;
			break;

		case 0x3FFB://angle
			motor_ch=1;
			break;

		case 0x3FFC://shake
			motor_ch=2;
			break;

		case 0x3FFD://angle
			motor_ch=3;
			break;
		default:
			motor_ch=-1;
			break;
		}

		if((motor_ch>=0) && (motor_ch<=3))
		{
			short pwr;
			pwr = atoi(mac.payload);
			hhb.write(motor_ch,pwr);
			motor_update = true;
			last_update_time[motor_ch] = millis();
			Serial.println_long(pwr,DEC);
		}

	}

	for(i=0;i<4;i++) 	{ 		if((millis() - last_update_time[i])>500){
			hhb.write(i,0);
			motor_update=true;
			last_update_time[i] = millis();
		}
	}
	if(motor_update) hhb.update();

	return;
}

  • 無線の設定(CH・PANID)を送信機側とあわせます。
  • hhb.init、hhb.attach、hhb.write、hhb.start、hhb.update関数でモーターの初期設定をしています。
  • swich文で、4台の送信機からそれぞれ値が送信されてきたら、何番のモーターを回すと、いう事を決めています。
  • atoi関数で、送られてきた文字データを数字に変換し、その数値をそのままモーターへ送ります。なので、振った力が大きければ大きいほどモーターは勢いよく回ります。また、手を上にあげていけば、モーターがだんだんと早く回り、下げるとゆっくり回ります。
  • このままだと、モーターが回りっぱなしなので、最後の通信から0.5秒たったら、モーターが止まる設定をしています。

動かしてみよう!

手を振ったらモーターが回る

手をあげたらモーターが回る

両方ともモーターが回り、止まってくれました♪

Laz-princess_footer