From c8d77fcb0b4cf78a6f01627ff99ad3b922b7ba5e Mon Sep 17 00:00:00 2001
From: kou029w <kou029w@gmail.com>
Date: Fri, 7 Sep 2012 07:10:41 +0900
Subject: [PATCH] update

---
 arduino/catchrobo2012/Motor.h           |  9 ++--
 arduino/catchrobo2012/WiiNun.cpp        |  2 +-
 arduino/catchrobo2012/WiiNun.h          |  2 +-
 arduino/catchrobo2012/catchrobo2012.ino | 67 +++++++++++++++++--------
 4 files changed, 50 insertions(+), 30 deletions(-)

diff --git a/arduino/catchrobo2012/Motor.h b/arduino/catchrobo2012/Motor.h
index 1d2a2cf..c1c20d7 100644
--- a/arduino/catchrobo2012/Motor.h
+++ b/arduino/catchrobo2012/Motor.h
@@ -6,17 +6,14 @@ Motor.h
 #ifndef Motor_h
 #define Motor_h
 
-#define STOP  0
-#define GO    1
-#define BACK  2
-#define BRAKE 3
-
 #if defined(ARDUINO) && ARDUINO >= 100
 #include <Arduino.h>
 #else
 #include <WProgram.h>
 #endif
 
+enum { STOP = 0, GO = 1, BACK = 2, BRAKE = 3};
+
 class Motor{
 public:
   void mode(byte mode);
@@ -28,4 +25,4 @@ private:
   byte _pin2;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/arduino/catchrobo2012/WiiNun.cpp b/arduino/catchrobo2012/WiiNun.cpp
index 7980874..d9682ee 100755
--- a/arduino/catchrobo2012/WiiNun.cpp
+++ b/arduino/catchrobo2012/WiiNun.cpp
@@ -34,4 +34,4 @@ void WiiNun::get(){
   Wire.beginTransmission(0x52);
   Wire.write(0x00);
   Wire.endTransmission();
-}
\ No newline at end of file
+}
diff --git a/arduino/catchrobo2012/WiiNun.h b/arduino/catchrobo2012/WiiNun.h
index 85c22be..c17826d 100755
--- a/arduino/catchrobo2012/WiiNun.h
+++ b/arduino/catchrobo2012/WiiNun.h
@@ -22,4 +22,4 @@ class WiiNun{
     byte _decodeByte(byte x);
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/arduino/catchrobo2012/catchrobo2012.ino b/arduino/catchrobo2012/catchrobo2012.ino
index b2cf00a..a0244e4 100755
--- a/arduino/catchrobo2012/catchrobo2012.ino
+++ b/arduino/catchrobo2012/catchrobo2012.ino
@@ -3,26 +3,28 @@
 #include "WiiNun.h"
 #include "Motor.h"
 
-#define PIN_LED 13
+const byte 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
+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;  // 垂直アームモーター
 
-#define PIN_SERVO_BASE         9
-#define PIN_SERVO_CATCHER     10
+const byte PIN_SERVO_BASE    =  9;  // 首ふりサーボ
+const byte PIN_SERVO_CATCHER = 10;  // キャッチャーサーボ
 
-#define PIN_PWR A3
-#define PIN_GND A2
+// ヌンチャクのためのピン
+const byte PIN_PWR = A3;
+const byte PIN_GND = A2;
 
-Servo base;
-Servo catcher;
-WiiNun WiiNun;
-Motor horMotor;
-Motor verMotor;
+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);
@@ -32,7 +34,6 @@ void setup(){
   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);
 }
@@ -55,30 +56,52 @@ void loop(){
   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にする
+  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(y>0x1F) baseDeg = 180 - atan2(y, x)*180/PI;  // 台座をスティックの角度にする
+    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(y*2);
-    baseDeg += x/32; // 台座をxに応じて相対移動
+    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ボタン押すとキャッチャーを離す