らずらいと姫の挑戦日記（第２３回）～加速度ｾﾝｻｰでﾓｰﾀを動かす～
2016-09-20
10月4日から開催するCEATECに出展するデモの開発の３回めです。
前回でモーターの準備が出来たので、加速度センサーの値を無線で送信して、それに連動してモーターを動かすデモを完成させました。
手を振ったら値を検出して、無線で送信するプログラム
[c]
#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);
}
[/c]
LazuriteIDEサンプルプログラムの「Axisio-serial」と「Welcome_SubGHz」を合体させます。
- 無線の設定（CH・PANID・HOST_ADDRESS）を受信機側とあわせます。
- 52~59行目が加速度センサーの値から色々と計算しているところになります。magは加速度の大きさを表しています。そこから、重力分の1Gをひいて振ったときの力 shake_forceを出しました。今回はプラスマイナス関係なく力だけを取りたかったので、fabsで絶対値に直しています。
- 95行目あたりで振った値が０．５より大きかったら、値×２０のデータを無線で送信するように設定しました。この値でモーターを回しています。
手をあげたら値を検出して、無線で送信するプログラム
振ったら回るプログラムとほぼ同じですが、95行目～98行目が違う部分です。
[c]
if(y > 30)
{
Print.init(send_data,sizeof(send_data));
Print.l(y,DEC);
[/c]
手の角度が30度位でモーターが回るようにしたいので、yの値が30以上だったら値を送信するように設定しました。
値を受信したらモーターが回るプログラム
LazuriteIDEサンプルプログラムの「Print_SubGHz」をベースに作っています。
[c]
#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;
}
[/c]
- 無線の設定（CH・PANID）を送信機側とあわせます。
- hhb.init、hhb.attach、hhb.write、hhb.start、hhb.update関数でモーターの初期設定をしています。
- swich文で、４台の送信機からそれぞれ値が送信されてきたら、何番のモーターを回すと、いう事を決めています。
- atoi関数で、送られてきた文字データを数字に変換し、その数値をそのままモーターへ送ります。なので、振った力が大きければ大きいほどモーターは勢いよく回ります。また、手を上にあげていけば、モーターがだんだんと早く回り、下げるとゆっくり回ります。
- このままだと、モーターが回りっぱなしなので、最後の通信から０．５秒たったら、モーターが止まる設定をしています。
動かしてみよう！
手を振ったらモーターが回る
手をあげたらモーターが回る
両方ともモーターが回り、止まってくれました♪
