From ac344bb427f9f209e023cb4f3c0b266c93febb0e Mon Sep 17 00:00:00 2001 From: kou029w Date: Thu, 30 Aug 2012 08:46:59 +0900 Subject: [PATCH] update --- arduino/catchrobo2012/Motor.cpp | 47 ++++++++ arduino/catchrobo2012/Motor.h | 31 ++++++ arduino/catchrobo2012/catchrobo2012 .ino | 132 ----------------------- arduino/catchrobo2012/catchrobo2012.ino | 89 +++++++++++++++ 4 files changed, 167 insertions(+), 132 deletions(-) create mode 100644 arduino/catchrobo2012/Motor.cpp create mode 100644 arduino/catchrobo2012/Motor.h delete mode 100644 arduino/catchrobo2012/catchrobo2012 .ino create mode 100644 arduino/catchrobo2012/catchrobo2012.ino diff --git a/arduino/catchrobo2012/Motor.cpp b/arduino/catchrobo2012/Motor.cpp new file mode 100644 index 0000000..33a1523 --- /dev/null +++ b/arduino/catchrobo2012/Motor.cpp @@ -0,0 +1,47 @@ +/* +Motor.cpp +(C)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +#if defined(ARDUINO) && ARDUINO >= 100 +#include +#else +#include +#endif +#include "Motor.h" + +void Motor::mode(byte mode){ + digitalWrite(Motor::_pin1, (mode>>0)&1); + digitalWrite(Motor::_pin2, (mode>>1)&1); +} + +void Motor::mode(byte mode, byte speed){ + switch(mode){ + case GO: + analogWrite(Motor::_pin1, speed); + digitalWrite(Motor::_pin2, LOW); + break; + case BACK: + analogWrite(Motor::_pin1, ~speed); + digitalWrite(Motor::_pin2, HIGH); + break; + default: + digitalWrite(Motor::_pin1, (mode>>0)&1); + digitalWrite(Motor::_pin2, (mode>>1)&1); + break; + } +} + +void Motor::speed(int speed){ + if(speed<0) Motor::mode(BACK, -speed); + else Motor::mode(GO, speed); +} + +void Motor::attach(byte pin1, byte pin2){ + Motor::_pin1 = pin1; + Motor::_pin2 = pin2; + pinMode(Motor::_pin1, OUTPUT); + pinMode(Motor::_pin2, OUTPUT); + digitalWrite(Motor::_pin1, LOW); + digitalWrite(Motor::_pin2, LOW); +} diff --git a/arduino/catchrobo2012/Motor.h b/arduino/catchrobo2012/Motor.h new file mode 100644 index 0000000..1d2a2cf --- /dev/null +++ b/arduino/catchrobo2012/Motor.h @@ -0,0 +1,31 @@ +/* +Motor.h +(C)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +#ifndef Motor_h +#define Motor_h + +#define STOP 0 +#define GO 1 +#define BACK 2 +#define BRAKE 3 + +#if defined(ARDUINO) && ARDUINO >= 100 +#include +#else +#include +#endif + +class Motor{ +public: + void mode(byte mode); + void mode(byte mode, byte speed); + void speed(int speed); + void attach(byte pin1, byte pin2); +private: + byte _pin1; + byte _pin2; +}; + +#endif \ No newline at end of file diff --git a/arduino/catchrobo2012/catchrobo2012 .ino b/arduino/catchrobo2012/catchrobo2012 .ino deleted file mode 100644 index 367ead6..0000000 --- a/arduino/catchrobo2012/catchrobo2012 .ino +++ /dev/null @@ -1,132 +0,0 @@ -#include -#include -#include - -#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); -} diff --git a/arduino/catchrobo2012/catchrobo2012.ino b/arduino/catchrobo2012/catchrobo2012.ino new file mode 100644 index 0000000..721fdbc --- /dev/null +++ b/arduino/catchrobo2012/catchrobo2012.ino @@ -0,0 +1,89 @@ +#include +#include +#include +#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); +}