113 lines
3.7 KiB
C++
Executable file
113 lines
3.7 KiB
C++
Executable file
#include <Servo.h>
|
|
#include <Wire.h>
|
|
#include "WiiNun.h"
|
|
#include "Motor.h"
|
|
|
|
const byte PIN_LED = 13;
|
|
|
|
const byte PIN_MOTOR_HORIZON_1 = 5; // 水平アームモーター
|
|
const byte PIN_MOTOR_HORIZON_2 = 4; // 水平アームモーター
|
|
const byte PIN_MOTOR_VERTICAL_1 = 6; // 垂直アームモーター
|
|
const byte PIN_MOTOR_VERTICAL_2 = 7; // 垂直アームモーター
|
|
|
|
const byte PIN_SERVO_BASE = 9; // 首ふりサーボ
|
|
const byte PIN_SERVO_CATCHER = 10; // キャッチャーサーボ
|
|
|
|
// ヌンチャクのためのピン
|
|
const byte PIN_PWR = A3;
|
|
const byte PIN_GND = A2;
|
|
|
|
Servo base; // 首ふりサーボ
|
|
Servo catcher; // キャッチャーサーボ
|
|
WiiNun WiiNun; // ヌンチャク
|
|
Motor horMotor; // 水平アームモーター
|
|
Motor verMotor; // 垂直アームモーター
|
|
|
|
void setup(){
|
|
pinMode(PIN_LED, OUTPUT);
|
|
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);
|
|
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;
|
|
// ヌンチャクおわり
|
|
|
|
int jx = (signed)(joyx-0x7F); // スティックの中心を0にする
|
|
int jy = (signed)(joyy-0x7F); // スティックの中心を0にする
|
|
// int ax = (signed)(accx-0x7F); // 使うときはコメントアウトすること
|
|
int ay = (signed)(accy-0x7F); // 水平状態を0にする
|
|
|
|
/* 速くて危険
|
|
static byte baseDeg = 90;
|
|
if(zbut){ // zボタン押しながら
|
|
digitalWrite(PIN_LED, HIGH);
|
|
if(jy>0x1F) baseDeg = 180 - atan2(jy, jx)*180/PI; // 首をスティックの角度にする
|
|
if(baseDeg < 10) baseDeg = 10;
|
|
else if(baseDeg > 170) baseDeg = 170;
|
|
}else{ // zボタン離しながら
|
|
digitalWrite(PIN_LED, LOW);
|
|
horMotor.speed(jy*2);
|
|
baseDeg += jx/32; // 首をjxに応じて相対移動
|
|
}
|
|
base.write(baseDeg);
|
|
*/
|
|
static unsigned int baseMicros = 1660; // 首のデフォの角度(だいたい90度にしたい。。。)
|
|
if(zbut){ // zボタン押しながら
|
|
digitalWrite(PIN_LED, HIGH);
|
|
baseMicros += jx/4; // 首をjxに応じて相対移動(x2速く)
|
|
}else{ // zボタン離しながら
|
|
digitalWrite(PIN_LED, LOW);
|
|
baseMicros += jx/8; // 首をjxに応じて相対移動
|
|
}
|
|
baseMicros = constrain(baseMicros, 544, 2400);
|
|
base.writeMicroseconds(baseMicros);
|
|
// Serial.println(baseMicros);
|
|
|
|
/* 粗い
|
|
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);
|
|
*/
|
|
int verMotorSpeed = 0; // -255-255
|
|
verMotorSpeed = constrain(ay*8, -255, 255);
|
|
verMotor.speed(verMotorSpeed);
|
|
|
|
int horMotorSpeed = 0; // -255-255
|
|
horMotorSpeed = constrain(jy*2, -255, 255);
|
|
horMotor.speed(horMotorSpeed);
|
|
|
|
byte catcherDeg = 135;
|
|
if(cbut){ // cボタン押すとキャッチャーを離す
|
|
catcherDeg = 45;
|
|
}
|
|
catcher.write(catcherDeg);
|
|
|
|
delay(50);
|
|
}
|