132 lines
3.3 KiB
C++
132 lines
3.3 KiB
C++
#include <Servo.h>
|
|
#include <Wire.h>
|
|
#include <WiiNun.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_HAND_1 10
|
|
#define PIN_SERVO_HAND_2 11
|
|
#define PIN_SERVO_HAND_3 12
|
|
|
|
#define STOP 0
|
|
#define GO 1
|
|
#define BACK 2
|
|
#define BRAKE 3
|
|
|
|
#define PIN_PWR A3
|
|
#define PIN_GND A2
|
|
|
|
int loop_cnt = 0;
|
|
Servo base;
|
|
Servo hand1;
|
|
Servo hand2;
|
|
Servo hand3;
|
|
WiiNun WiiNun;
|
|
|
|
// 台座の角度
|
|
byte theta = 90; // degree
|
|
|
|
void hMotorSpeed(char sspeed){
|
|
byte speed = abs(sspeed)*2;
|
|
if(sspeed<0){
|
|
analogWrite(PIN_MOTOR_HORIZON_1, 0xFF-speed);
|
|
digitalWrite(PIN_MOTOR_HORIZON_2, HIGH);
|
|
}else{
|
|
analogWrite(PIN_MOTOR_HORIZON_1, speed);
|
|
digitalWrite(PIN_MOTOR_HORIZON_2, LOW);
|
|
}
|
|
}
|
|
|
|
void vMotorSpeed(char sspeed){
|
|
byte speed = abs(sspeed)*2;
|
|
if(sspeed<0){
|
|
analogWrite(PIN_MOTOR_VERTICAL_1, 0xFF-speed);
|
|
digitalWrite(PIN_MOTOR_VERTICAL_2, HIGH);
|
|
}else{
|
|
analogWrite(PIN_MOTOR_VERTICAL_1, speed);
|
|
digitalWrite(PIN_MOTOR_VERTICAL_2, LOW);
|
|
}
|
|
}
|
|
|
|
// 0-2piで返すatan
|
|
double atan(double x, double y){
|
|
if(x>=0 && y<0) return atan(y/x)+2*PI;
|
|
else if(x<0) return atan(y/x)+PI;
|
|
else return atan(y/x);
|
|
}
|
|
|
|
void setup(){
|
|
pinMode(PIN_PWR, OUTPUT);
|
|
pinMode(PIN_GND, OUTPUT);
|
|
digitalWrite(PIN_PWR, HIGH);
|
|
digitalWrite(PIN_GND, LOW);
|
|
delay(100);
|
|
WiiNun.begin();
|
|
base.attach(PIN_SERVO_BASE);
|
|
hand1.attach(PIN_SERVO_HAND_1);
|
|
hand2.attach(PIN_SERVO_HAND_2);
|
|
hand3.attach(PIN_SERVO_HAND_3);
|
|
// Serial.begin(19200);
|
|
pinMode(PIN_LED , OUTPUT);
|
|
pinMode(PIN_MOTOR_HORIZON_1 , OUTPUT);
|
|
pinMode(PIN_MOTOR_HORIZON_2 , OUTPUT);
|
|
pinMode(PIN_MOTOR_VERTICAL_1, OUTPUT);
|
|
pinMode(PIN_MOTOR_VERTICAL_2, OUTPUT);
|
|
}
|
|
|
|
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 ay = (char)(accy-0x7F);
|
|
digitalWrite(PIN_LED, LOW);
|
|
if(zbut){
|
|
digitalWrite(PIN_LED, HIGH);
|
|
if(y>0x1F) theta = atan(x,y)*180/PI;
|
|
else theta -= x/32;
|
|
if(theta < 10) theta = 10;
|
|
else if(theta > 170) theta = 170;
|
|
base.write(theta);
|
|
}else{
|
|
hMotorSpeed(y);
|
|
}
|
|
vMotorSpeed(ay);
|
|
|
|
byte catchDegree = 90;
|
|
if(cbut){
|
|
catchDegree = 30;
|
|
}
|
|
hand1.write(catchDegree);
|
|
hand2.write(catchDegree);
|
|
hand3.write(catchDegree);
|
|
|
|
// Serial.print("theta: "); Serial.print(theta);
|
|
// Serial.print("\tjoyx: "); Serial.print((byte)joyx,DEC);
|
|
// Serial.print("\tjoyy: "); Serial.print((byte)joyy,DEC);
|
|
// Serial.print("\taccx: "); Serial.print((byte)accx,DEC);
|
|
// Serial.print("\taccy: "); Serial.print((byte)accy,DEC);
|
|
// Serial.print("\taccz: "); Serial.print((byte)accz,DEC);
|
|
// Serial.print("\tzbut: "); Serial.print((byte)zbut,DEC);
|
|
// Serial.print("\tcbut: "); Serial.println((byte)cbut,DEC);
|
|
|
|
delay(10);
|
|
}
|