90 lines
2.2 KiB
Arduino
90 lines
2.2 KiB
Arduino
|
#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
|
||
|
|
|
||
|
|
int loop_cnt = 0;
|
||
|
|
Servo base;
|
||
|
|
Servo catcher;
|
||
|
|
WiiNun wiiNun;
|
||
|
|
Motor horMotor;
|
||
|
|
Motor verMotor;
|
||
|
|
|
||
|
|
byte baseDeg = 90;
|
||
|
|
byte catcherDeg = 90;
|
||
|
|
|
||
|
|
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);
|
||
|
|
char y = (char)(joyy-0x7F);
|
||
|
|
// char ax = (char)(accx-0x7F);
|
||
|
|
char ay = (char)(accy-0x7F);
|
||
|
|
|
||
|
|
digitalWrite(PIN_LED, LOW);
|
||
|
|
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{
|
||
|
|
horMotor.speed(y);
|
||
|
|
baseDeg += x/32; // 台座をxに応じて相対移動
|
||
|
|
}
|
||
|
|
|
||
|
|
if(ay > +0x10) verMotor.speed(+0xCC);
|
||
|
|
if(ay < -0x10) verMotor.speed(-0xCC);
|
||
|
|
if(ay > +0x20) verMotor.mode(GO);
|
||
|
|
if(ay < -0x20) verMotor.mode(BACK);
|
||
|
|
|
||
|
|
catcherDeg = 90;
|
||
|
|
if(cbut){
|
||
|
|
catcherDeg = 15;
|
||
|
|
}
|
||
|
|
|
||
|
|
base.attach(baseDeg);
|
||
|
|
catcher.write(catcherDeg);
|
||
|
|
delay(10);
|
||
|
|
}
|