90 lines
2.4 KiB
C++
Executable file
90 lines
2.4 KiB
C++
Executable file
#include <Servo.h>
|
|
#include <Wire.h>
|
|
#include "WiiNun.h"
|
|
#include "Motor.h"
|
|
|
|
#define PIN_LED 13
|
|
|
|
#define PIN_MOTOR_HORIZON_1 5
|
|
#define PIN_MOTOR_HORIZON_2 4
|
|
#define PIN_MOTOR_VERTICAL_1 6
|
|
#define PIN_MOTOR_VERTICAL_2 7
|
|
|
|
#define PIN_SERVO_BASE 9
|
|
#define PIN_SERVO_CATCHER 10
|
|
|
|
#define PIN_PWR A3
|
|
#define PIN_GND A2
|
|
|
|
Servo base;
|
|
Servo catcher;
|
|
WiiNun WiiNun;
|
|
Motor horMotor;
|
|
Motor verMotor;
|
|
|
|
void setup(){
|
|
pinMode(PIN_PWR, OUTPUT);
|
|
pinMode(PIN_GND, OUTPUT);
|
|
digitalWrite(PIN_PWR, HIGH);
|
|
digitalWrite(PIN_GND, LOW);
|
|
delay(100);
|
|
// Serial.begin(19200);
|
|
WiiNun.begin();
|
|
base.attach(PIN_SERVO_BASE);
|
|
catcher.attach(PIN_SERVO_CATCHER);
|
|
pinMode(PIN_LED, OUTPUT);
|
|
horMotor.attach(PIN_MOTOR_HORIZON_1, PIN_MOTOR_HORIZON_2);
|
|
verMotor.attach(PIN_MOTOR_VERTICAL_1, PIN_MOTOR_VERTICAL_2);
|
|
}
|
|
|
|
void loop(){
|
|
// ヌンチャクを読む
|
|
WiiNun.get();
|
|
byte joyx = WiiNun.data[0];
|
|
byte joyy = WiiNun.data[1];
|
|
byte accx = WiiNun.data[2];
|
|
byte accy = WiiNun.data[3];
|
|
byte accz = WiiNun.data[4];
|
|
byte zbut = ((~WiiNun.data[5] >> 0) & 1);
|
|
byte cbut = ((~WiiNun.data[5] >> 1) & 1);
|
|
if((WiiNun.data[5] >> 2) & 1) accx += 2;
|
|
if((WiiNun.data[5] >> 3) & 1) accx += 1;
|
|
if((WiiNun.data[5] >> 4) & 1) accy += 2;
|
|
if((WiiNun.data[5] >> 5) & 1) accy += 1;
|
|
if((WiiNun.data[5] >> 6) & 1) accz += 2;
|
|
if((WiiNun.data[5] >> 7) & 1) accz += 1;
|
|
// ヌンチャクおわり
|
|
|
|
char x = (char)(joyx-0x7F); // スティックの中心を0にする
|
|
char y = (char)(joyy-0x7F); // スティックの中心を0にする
|
|
// char ax = (char)(accx-0x7F);
|
|
char ay = (char)(accy-0x7F); // 水平状態を0にする
|
|
|
|
static byte baseDeg = 90;
|
|
if(zbut){ // zボタン押しながら
|
|
digitalWrite(PIN_LED, HIGH);
|
|
if(y>0x1F) baseDeg = 180 - atan2(y, x)*180/PI; // 台座をスティックの角度にする
|
|
if(baseDeg < 10) baseDeg = 10;
|
|
else if(baseDeg > 170) baseDeg = 170;
|
|
}else{ // zボタン離しながら
|
|
digitalWrite(PIN_LED, LOW);
|
|
horMotor.speed(y*2);
|
|
baseDeg += x/32; // 台座をxに応じて相対移動
|
|
}
|
|
base.write(baseDeg);
|
|
|
|
if(0);
|
|
else if(ay > +0x20) verMotor.mode(GO);
|
|
else if(ay > +0x10) verMotor.speed(+0xCC);
|
|
else if(ay < -0x20) verMotor.mode(BACK);
|
|
else if(ay < -0x10) verMotor.speed(-0xCC);
|
|
else verMotor.mode(STOP);
|
|
|
|
byte catcherDeg = 135;
|
|
if(cbut){ // cボタン押すとキャッチャーを離す
|
|
catcherDeg = 45;
|
|
}
|
|
catcher.write(catcherDeg);
|
|
|
|
delay(50);
|
|
}
|