From 1017975df77aae43f5fc7433b609145a37663521 Mon Sep 17 00:00:00 2001 From: kou029w Date: Tue, 28 Aug 2012 22:39:10 +0900 Subject: [PATCH] =?UTF-8?q?=E5=90=8C=E6=9C=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../AnalogReadSerial_x6.ino | 18 + Arduino/Default/Default.ino | 12 + .../DigitalReadSerial_x6.ino | 18 + .../DigitalReadSerial_x8.ino | 30 ++ Arduino/PSConSPI/PSConSPI.ino | 64 +++ Arduino/ServoSet/ServoSet.ino | 33 ++ .../ServoSetMicroseconds.ino | 33 ++ Arduino/catchrobo/catchrobo.ino | 181 +++++++++ Arduino/catchrobo2012/catchrobo2012 .ino | 132 +++++++ Arduino/hanzo1/hanzo1.pde | 364 ++++++++++++++++++ Arduino/hanzo2/hanzo2.ino | 292 ++++++++++++++ Arduino/libraries/PSCon/PSCon.cpp | 62 +++ Arduino/libraries/PSCon/PSCon.h | 31 ++ .../examples/PSConSerial/PSConSerial.ino | 63 +++ Arduino/libraries/PSCon/keywords.txt | 14 + Arduino/libraries/WiiNun/WiiNun.cpp | 37 ++ Arduino/libraries/WiiNun/WiiNun.h | 25 ++ .../examples/WiiNunSerial/WiiNunSerial.ino | 59 +++ Arduino/libraries/WiiNun/keywords.txt | 11 + Arduino/mini_rhinoceros/mini_rhinoceros.pde | 40 ++ 20 files changed, 1519 insertions(+) create mode 100644 Arduino/AnalogReadSerial_x6/AnalogReadSerial_x6.ino create mode 100644 Arduino/Default/Default.ino create mode 100644 Arduino/DigitalReadSerial_x6/DigitalReadSerial_x6.ino create mode 100644 Arduino/DigitalReadSerial_x8/DigitalReadSerial_x8.ino create mode 100644 Arduino/PSConSPI/PSConSPI.ino create mode 100644 Arduino/ServoSet/ServoSet.ino create mode 100644 Arduino/ServoSetMicroseconds/ServoSetMicroseconds.ino create mode 100755 Arduino/catchrobo/catchrobo.ino create mode 100644 Arduino/catchrobo2012/catchrobo2012 .ino create mode 100644 Arduino/hanzo1/hanzo1.pde create mode 100644 Arduino/hanzo2/hanzo2.ino create mode 100644 Arduino/libraries/PSCon/PSCon.cpp create mode 100644 Arduino/libraries/PSCon/PSCon.h create mode 100644 Arduino/libraries/PSCon/examples/PSConSerial/PSConSerial.ino create mode 100755 Arduino/libraries/PSCon/keywords.txt create mode 100644 Arduino/libraries/WiiNun/WiiNun.cpp create mode 100644 Arduino/libraries/WiiNun/WiiNun.h create mode 100644 Arduino/libraries/WiiNun/examples/WiiNunSerial/WiiNunSerial.ino create mode 100755 Arduino/libraries/WiiNun/keywords.txt create mode 100644 Arduino/mini_rhinoceros/mini_rhinoceros.pde diff --git a/Arduino/AnalogReadSerial_x6/AnalogReadSerial_x6.ino b/Arduino/AnalogReadSerial_x6/AnalogReadSerial_x6.ino new file mode 100644 index 0000000..e215e26 --- /dev/null +++ b/Arduino/AnalogReadSerial_x6/AnalogReadSerial_x6.ino @@ -0,0 +1,18 @@ +void setup() { + Serial.begin(9600); +} + +void loop() { + Serial.print(analogRead(0)); + Serial.print('\t'); + Serial.print(analogRead(1)); + Serial.print('\t'); + Serial.print(analogRead(2)); + Serial.print('\t'); + Serial.print(analogRead(3)); + Serial.print('\t'); + Serial.print(analogRead(4)); + Serial.print('\t'); + Serial.print(analogRead(5)); + Serial.print('\n'); +} diff --git a/Arduino/Default/Default.ino b/Arduino/Default/Default.ino new file mode 100644 index 0000000..d1d0b10 --- /dev/null +++ b/Arduino/Default/Default.ino @@ -0,0 +1,12 @@ +// (C)2012 kou029w - MIT License + +int analogRead(uint8_t pin, uint8_t max){ + return ((long)analogRead(pin)*max)>>10; +} + +void setup(){ + +} +void loop(){ + +} diff --git a/Arduino/DigitalReadSerial_x6/DigitalReadSerial_x6.ino b/Arduino/DigitalReadSerial_x6/DigitalReadSerial_x6.ino new file mode 100644 index 0000000..7c3424d --- /dev/null +++ b/Arduino/DigitalReadSerial_x6/DigitalReadSerial_x6.ino @@ -0,0 +1,18 @@ +void setup() { + Serial.begin(9600); +} + +void loop() { + Serial.print(digitalRead(14)); + Serial.print('\t'); + Serial.print(digitalRead(15)); + Serial.print('\t'); + Serial.print(digitalRead(16)); + Serial.print('\t'); + Serial.print(digitalRead(17)); + Serial.print('\t'); + Serial.print(digitalRead(18)); + Serial.print('\t'); + Serial.print(digitalRead(19)); + Serial.print('\n'); +} diff --git a/Arduino/DigitalReadSerial_x8/DigitalReadSerial_x8.ino b/Arduino/DigitalReadSerial_x8/DigitalReadSerial_x8.ino new file mode 100644 index 0000000..4b1524d --- /dev/null +++ b/Arduino/DigitalReadSerial_x8/DigitalReadSerial_x8.ino @@ -0,0 +1,30 @@ +void setup() { + pinMode(12, INPUT); + pinMode(13, INPUT); + pinMode(14, INPUT); + pinMode(15, INPUT); + pinMode(16, INPUT); + pinMode(17, INPUT); + pinMode(18, INPUT); + pinMode(19, INPUT); + Serial.begin(9600); +} + +void loop() { + Serial.print(digitalRead(12)); + Serial.print('\t'); + Serial.print(digitalRead(13)); + Serial.print('\t'); + Serial.print(digitalRead(14)); + Serial.print('\t'); + Serial.print(digitalRead(15)); + Serial.print('\t'); + Serial.print(digitalRead(16)); + Serial.print('\t'); + Serial.print(digitalRead(17)); + Serial.print('\t'); + Serial.print(digitalRead(18)); + Serial.print('\t'); + Serial.print(digitalRead(19)); + Serial.print('\n'); +} diff --git a/Arduino/PSConSPI/PSConSPI.ino b/Arduino/PSConSPI/PSConSPI.ino new file mode 100644 index 0000000..9955d17 --- /dev/null +++ b/Arduino/PSConSPI/PSConSPI.ino @@ -0,0 +1,64 @@ +/* +PSConSPI +(c)2011 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] + +PSCon Arduino +CLK 13 SCK +SEL 10 SS +CMD 11 MOSI +DAT 12 MISO +*/ + +#include + +void get(byte data[8]){ + digitalWrite(10, LOW); + SPI.transfer(0x01); + for(byte i=0; i<8; i++){ + data[i] = SPI.transfer(data[i]); + } + digitalWrite(10, HIGH); +} + +void setup(){ + SPI.setBitOrder(LSBFIRST); + SPI.setDataMode(SPI_MODE3); + SPI.setClockDivider(SPI_CLOCK_DIV128); + SPI.begin(); + Serial.begin(9600); +} + +void loop(){ + byte data[] = {0x42,0,0,0,0,0,0,0}; + get(data); + if(data[0] == 0x73){ // デジコン:0x41, アナコン:0x73 + Serial.print("R(x,y):("); + Serial.print(data[4], DEC); // 上が0x00, 下が0xFF + Serial.print(","); + Serial.print(data[5], DEC); // 左が0x00, 右が0xFF + Serial.print(") "); + Serial.print("L(x,y):("); + Serial.print(data[6], DEC); // 上が0x00, 下が0xFF + Serial.print(","); + Serial.print(data[7], DEC); // 左が0x00, 右が0xFF + Serial.print(") "); + } + if(~data[2] & 0x01<<0)Serial.print("sel "); + if(~data[2] & 0x01<<1)Serial.print("L3 "); + if(~data[2] & 0x01<<2)Serial.print("R3 "); + if(~data[2] & 0x01<<3)Serial.print("sta "); + if(~data[2] & 0x01<<4)Serial.print("^ "); + if(~data[2] & 0x01<<5)Serial.print("> "); + if(~data[2] & 0x01<<6)Serial.print("v "); + if(~data[2] & 0x01<<7)Serial.print("< "); + if(~data[3] & 0x01<<0)Serial.print("L2 "); + if(~data[3] & 0x01<<1)Serial.print("R2 "); + if(~data[3] & 0x01<<2)Serial.print("L1 "); + if(~data[3] & 0x01<<3)Serial.print("R1 "); + if(~data[3] & 0x01<<4)Serial.print("% "); + if(~data[3] & 0x01<<5)Serial.print("O "); + if(~data[3] & 0x01<<6)Serial.print("X "); + if(~data[3] & 0x01<<7)Serial.print("# "); + Serial.println(); + delay(16); +} diff --git a/Arduino/ServoSet/ServoSet.ino b/Arduino/ServoSet/ServoSet.ino new file mode 100644 index 0000000..8b8a880 --- /dev/null +++ b/Arduino/ServoSet/ServoSet.ino @@ -0,0 +1,33 @@ +#include + +Servo servo1; +Servo servo2; + +void setup() { + servo1.attach(9); + servo2.attach(10); + Serial.begin(9600); + Serial.print("format : [0-180][ab]"); +} + +void loop() { + static int v = 0; + + if ( Serial.available() ) { + char ch = Serial.read(); + + switch(ch) { + case '0'...'9': + v = v * 10 + ch - '0'; + break; + case 'a': + servo1.write(v); + v = 0; + break; + case 'b': + servo2.write(v); + v = 0; + break; + } + } +} diff --git a/Arduino/ServoSetMicroseconds/ServoSetMicroseconds.ino b/Arduino/ServoSetMicroseconds/ServoSetMicroseconds.ino new file mode 100644 index 0000000..49c6132 --- /dev/null +++ b/Arduino/ServoSetMicroseconds/ServoSetMicroseconds.ino @@ -0,0 +1,33 @@ +#include + +Servo servo1; +Servo servo2; + +void setup() { + servo1.attach(9); + servo2.attach(10); + Serial.begin(9600); + Serial.print("format : [0-9]+[ab]"); +} + +void loop() { + static int v = 0; + + if ( Serial.available()) { + char ch = Serial.read(); + + switch(ch) { + case '0'...'9': + v = v * 10 + ch - '0'; + break; + case 'a': + servo1.writeMicroseconds(v); + v = 0; + break; + case 'b': + servo2.writeMicroseconds(v); + v = 0; + break; + } + } +} diff --git a/Arduino/catchrobo/catchrobo.ino b/Arduino/catchrobo/catchrobo.ino new file mode 100755 index 0000000..60140e6 --- /dev/null +++ b/Arduino/catchrobo/catchrobo.ino @@ -0,0 +1,181 @@ +#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 pwrpin PORTC3 +#define gndpin PORTC2 + +byte nunchuck_buf[6]; +int loop_cnt = 0; +Servo base; +Servo hand1; +Servo hand2; +Servo hand3; + +// 台座の角度 +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); + } +} + +void nunchuckInit(){ + DDRC |= _BV(pwrpin) | _BV(gndpin); + PORTC &=~ _BV(gndpin); + PORTC |= _BV(pwrpin); + delay(100); + Wire.begin(); + Wire.beginTransmission(0x52); + Wire.write(0x40);// sends memory address + Wire.write(0x00);// sends sent a zero + Wire.endTransmission();// stop transmitting +} + +// Send a request for data to the nunchuck +// was "send_zero()" +void nunchuck_send_request() +{ + Wire.beginTransmission(0x52);// transmit to device 0x52 + Wire.write(0x00);// sends one byte + Wire.endTransmission();// stop transmitting +} + +// Encode data to format that most wiimote drivers except +// only needed if you use one of the regular wiimote drivers +char nunchuk_decode_byte (char x) +{ + x = (x ^ 0x17) + 0x17; + return x; +} + +// Receive data back from the nunchuck, +// returns 1 on successful read. returns 0 on failure +byte nunchuckGet(){ + int cnt=0; + Wire.requestFrom (0x52, 6);// request data from nunchuck + while (Wire.available()) { + // receive byte as an integer + nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.read()); + cnt++; + } + nunchuck_send_request(); // send request for next data payload + // If we recieved the 6 bytes, then go print them + if (cnt >= 5) { + return 1; // success + } + return 0; //failure +} + +// 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(){ + 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); + nunchuckInit(); + 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(){ + nunchuckGet(); + byte joyx = nunchuck_buf[0]; + byte joyy = nunchuck_buf[1]; + byte accx = nunchuck_buf[2]; + byte accy = nunchuck_buf[3]; + byte accz = nunchuck_buf[4]; + byte zbut = ((~nunchuck_buf[5] >> 0) & 1); + byte cbut = ((~nunchuck_buf[5] >> 1) & 1); + if ((nunchuck_buf[5] >> 2) & 1) + accx += 2; + if ((nunchuck_buf[5] >> 3) & 1) + accx += 1; + if ((nunchuck_buf[5] >> 4) & 1) + accy += 2; + if ((nunchuck_buf[5] >> 5) & 1) + accy += 1; + if ((nunchuck_buf[5] >> 6) & 1) + accz += 2; + if ((nunchuck_buf[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..367ead6 --- /dev/null +++ b/Arduino/catchrobo2012/catchrobo2012 .ino @@ -0,0 +1,132 @@ +#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/hanzo1/hanzo1.pde b/Arduino/hanzo1/hanzo1.pde new file mode 100644 index 0000000..a50b2cf --- /dev/null +++ b/Arduino/hanzo1/hanzo1.pde @@ -0,0 +1,364 @@ +/* +競技用ランサーロボット + 半蔵 1.0 + (c)2011 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] + */ + +/* + <前> + +[0 1 2 3 4 5] + | + | + []---*---[] + | , + |/ + []---#---[] + +*/ + +#define PIN_ANALOG_SENSOR_0 5 +#define PIN_ANALOG_SENSOR_1 4 +#define PIN_ANALOG_SENSOR_2 3 +#define PIN_ANALOG_SENSOR_3 2 +#define PIN_ANALOG_SENSOR_4 1 +#define PIN_ANALOG_SENSOR_5 0 + +#define PIN_SERVO_HANDLE 10 +#define PIN_SERVO_LANCE 9 +#define PIN_MOTOR_RIGHT_SPEED 3 +#define PIN_MOTOR_RIGHT_0 2 +#define PIN_MOTOR_RIGHT_1 8 +#define PIN_MOTOR_LEFT_SPEED 5 +#define PIN_MOTOR_LEFT_0 7 +#define PIN_MOTOR_LEFT_1 6 // 0:HIGH, 1:LOW => 正転 / 0:LOW, 1:HIGH => 逆転 + +#define STOP 0x00 +#define GO 0b01 +#define BACK 0b10 +#define BRAKE 0b11 + +#define SPEED_MAX 255 +#define SPEED_DEFAULT 255 + +#define HANDLE_DEFAULT 87 //度 +#define HANDLE_MODE_LEFT -25 //度 +#define HANDLE_MODE_RIGHT 20 //度 + +#define LANCE_DEFAULT 95 //度 + +#define SENSOR_LIMIT 95 //センサーのしきい値 +// センサー < SENSOR_LIMIT => 白 + +#define MODE_STOP 0x00 +#define MODE_TRACE 0x10 +#define MODE_RIGHT 0x21 // 右カーブ +#define MODE_LEFT 0x22 // 左カーブ + +#define TARGET_NONE 0x40 +#define TARGET_PARALLEL 0x41 +#define TARGET_VERTICAL 0x42 +#define TARGET_CYLINDER 0x43 + +#define MASK_MODE_TRACE 0b011110 +#define MASK_MODE_RIGHT 0b001000 +#define MASK_MODE_LEFT 0b000100 +#define MASK_CHECK_MARKER 0b100001 +#define MASK_CHECK_MARKER_RIGHT 0b000001 +#define MASK_CHECK_MARKER_LEFT 0b100000 + +#define TIMER_INTERVAL 20 // timer()の実行間隔(ms) + +//#define DEBUG //デバッグ有効(30秒停止機能) +//#define LINE_BLACK //白地・黒ライン有効 + +/**************************************/ + +#include +// #include + +/**************************************/ + +Servo servo_handle; +Servo servo_lance; + +byte mode; +byte sensor_old; +//byte marker_old; +//byte target; +byte lance_old; +//byte marker; + +/**************************************/ + +byte sensor(){ + byte sensor_result = 0; + +#ifdef LINE_BLACK + if(analogRead(PIN_ANALOG_SENSOR_0) >= SENSOR_LIMIT){ //黒 + sensor_result += 0b100000 ; + } + if(analogRead(PIN_ANALOG_SENSOR_1) >= SENSOR_LIMIT){ + sensor_result += 0b010000 ; + } + if(analogRead(PIN_ANALOG_SENSOR_2) >= SENSOR_LIMIT){ + sensor_result += 0b001000 ; + } + if(analogRead(PIN_ANALOG_SENSOR_3) >= SENSOR_LIMIT){ + sensor_result += 0b000100 ; + } + if(analogRead(PIN_ANALOG_SENSOR_4) >= SENSOR_LIMIT){ + sensor_result += 0b000010 ; + } + if(analogRead(PIN_ANALOG_SENSOR_5) >= SENSOR_LIMIT){ + sensor_result += 0b000001 ; + } +#else + if(analogRead(PIN_ANALOG_SENSOR_0) < SENSOR_LIMIT){ //白 + sensor_result += 0b100000 ; + } + if(analogRead(PIN_ANALOG_SENSOR_1) < SENSOR_LIMIT){ + sensor_result += 0b010000 ; + } + if(analogRead(PIN_ANALOG_SENSOR_2) < SENSOR_LIMIT){ + sensor_result += 0b001000 ; + } + if(analogRead(PIN_ANALOG_SENSOR_3) < SENSOR_LIMIT){ + sensor_result += 0b000100 ; + } + if(analogRead(PIN_ANALOG_SENSOR_4) < SENSOR_LIMIT){ + sensor_result += 0b000010 ; + } + if(analogRead(PIN_ANALOG_SENSOR_5) < SENSOR_LIMIT){ + sensor_result += 0b000001 ; + } +#endif + + return sensor_result; +} + +/* +bool check_marker(){ + bool check_marker_result = false; + if(sensor(MASK_CHECK_MARKER)) check_marker_result = true; + return check_marker_result; +} + +bool check_marker_right(){ + bool check_marker_result = false; + if(sensor(MASK_CHECK_MARKER_RIGHT)) check_marker_result = true; + return check_marker_result; +} + +bool check_marker_left(){ + bool check_marker_result = false; + if(sensor(MASK_CHECK_MARKER_LEFT)) check_marker_result = true; + return check_marker_result; +} +*/ + +void motorSpeed(byte motor_left_speed, byte motor_right_speed){ + analogWrite(PIN_MOTOR_LEFT_SPEED, motor_left_speed); + analogWrite(PIN_MOTOR_RIGHT_SPEED, motor_right_speed); +} + +void motorMode(byte motor_left_mode, byte motor_right_mode){ + digitalWrite(PIN_MOTOR_LEFT_0, ( motor_left_mode >> 0 ) & 0x01 ); + digitalWrite(PIN_MOTOR_LEFT_1, ( motor_left_mode >> 1 ) & 0x01 ); + digitalWrite(PIN_MOTOR_RIGHT_0, ( motor_right_mode >> 0 ) & 0x01 ); + digitalWrite(PIN_MOTOR_RIGHT_1, ( motor_right_mode >> 1 ) & 0x01 ); +} + +void handle(char handle_angle){ + servo_handle.write(HANDLE_DEFAULT + handle_angle); +} + +void lance(char lance_angle){ + servo_lance.write(LANCE_DEFAULT - lance_angle); +} + +#ifdef DEBUG +void timer(unsigned long millis_now){ + if(millis_now > 30 * 1000){ + mode = MODE_STOP; + } +} +#endif + +void setup() { + + pinMode(PIN_ANALOG_SENSOR_0, INPUT); + pinMode(PIN_ANALOG_SENSOR_1, INPUT); + pinMode(PIN_ANALOG_SENSOR_2, INPUT); + pinMode(PIN_ANALOG_SENSOR_3, INPUT); + pinMode(PIN_ANALOG_SENSOR_4, INPUT); + pinMode(PIN_ANALOG_SENSOR_5, INPUT); + + pinMode(PIN_MOTOR_LEFT_SPEED, OUTPUT); + pinMode(PIN_MOTOR_LEFT_0, OUTPUT); + pinMode(PIN_MOTOR_LEFT_1, OUTPUT); + + pinMode(PIN_MOTOR_RIGHT_SPEED, OUTPUT); + pinMode(PIN_MOTOR_RIGHT_0, OUTPUT); + pinMode(PIN_MOTOR_RIGHT_1, OUTPUT); + + motorSpeed(0, 0); + motorMode(STOP, STOP); + + servo_handle.attach(PIN_SERVO_HANDLE); + handle(0); + + servo_lance.attach(PIN_SERVO_LANCE); + lance(0); + + mode = MODE_STOP; + + // target = TARGET_NONE; + // marker =0; + + /* + MsTimer2::set(TIMER_INTERVAL, timer); + MsTimer2::start(); + */ + + //-- + + delay(1000); + // Serial.begin(9600); + mode = MODE_TRACE; + handle(0); + motorMode(GO, GO); + motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT); + +} + +/**************************************/ + +void loop() { + sensor_old = sensor(); + + switch(mode){ + case MODE_STOP: + lance(0); + handle(0); + motorMode(BRAKE, BRAKE); + motorSpeed(0, 0); + break; + + case MODE_TRACE: + + static unsigned long millis_lance = 0; + if(millis() - millis_lance > 70){ + millis_lance = millis(); + + switch(lance_old){ + case 39: + lance_old = 29; + break; + case 29: + lance_old = 39; + break; + } + } + + if( (sensor_old >> 5) & 0x01){ + lance_old = -33; + } + + if( (sensor_old >> 0) & 0x01){ + lance_old = 39; + } + + lance(lance_old); + // handle(0); + // motorMode(GO, GO); + // motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT); + /* + switch(sensor(MASK_CHECK_MARKER)){ + case 0b000001: + marker = 0x01; + break; + case 0b100000: + marker = 0x02; + break; + } + */ + switch(sensor_old){ + // case 0b000000: + // handle(0); + // motorMode(GO, GO); + // motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT); + // break; + + case 0b001000: + handle(-3); + motorMode(GO, GO); + motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT); + break; + + case 0b000100: + handle(3); + motorMode(GO, GO); + motorSpeed(SPEED_DEFAULT,SPEED_DEFAULT); + break; + + case 0b010000: + // delay(0); + mode = MODE_LEFT; + // handle(HANDLE_MODE_LEFT); + // motorMode(GO, GO); + // motorSpeed(SPEED_DEFAULT*0.7, SPEED_DEFAULT); + break; + + case 0b000010: + handle(12); + motorMode(GO, GO); + motorSpeed(SPEED_DEFAULT,SPEED_DEFAULT); + //mode = MODE_RIGHT; + break; + + } + + break; + + case MODE_LEFT: + + switch(sensor_old){ + case 0b010000: + handle(HANDLE_MODE_LEFT); + motorMode(BRAKE, GO); + motorSpeed(0, SPEED_DEFAULT); + break; +/* case 0b001000: + handle(HANDLE_MODE_LEFT); + motorMode(GO, GO); + motorSpeed(SPEED_DEFAULT*0.7, SPEED_DEFAULT); + break; + */ + case 0b000100: + mode = MODE_TRACE; + break; + } + break; + /* + case MODE_RIGHT: + handle(HANDLE_MODE_RIGHT); + motorMode(GO, GO); + motorSpeed(SPEED_DEFAULT, SPEED_DEFAULT*0.8); + switch(sensor(MASK_MODE_RIGHT)){ + case 0b001000: + mode = MODE_TRACE; + break; + } + break; + */ + } +#ifdef DEBUG + static unsigned long millis_old = 0; + if(millis() - millis_old > TIMER_INTERVAL){ + millis_old = millis(); + timer(millis_old); + } +#endif +} + diff --git a/Arduino/hanzo2/hanzo2.ino b/Arduino/hanzo2/hanzo2.ino new file mode 100644 index 0000000..eca008d --- /dev/null +++ b/Arduino/hanzo2/hanzo2.ino @@ -0,0 +1,292 @@ +/* +競技用ランサーロボット +半蔵 2.0 +(C)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +/* + <前> + +[7 654321 0] + | + | +[]---*----[] + | / + |/ +[]---#----[] +*/ + +//#define LINE_BLACK //白地・黒ライン有効 +#define DEBUG //デバッグ有効 + +#define TARGET_NONE_COUNT 85 + +#define LANCE_ANGLE0 0 //度 +#define LANCE_ANGLE1 43 +#define LANCE_ANGLE2 54 +#define LANCE_ANGLE3 -33 +#define LANCE_ANGLE4 34 +#define LANCE_INTERVAL 70 //ms + +#define PIN_LED 4 +#define PIN_ROT 2 + +#define PIN_SENSOR_0 12 +#define PIN_SENSOR_1 13 +#define PIN_SENSOR_2 14 +#define PIN_SENSOR_3 15 +#define PIN_SENSOR_4 16 +#define PIN_SENSOR_5 17 +#define PIN_SENSOR_6 18 +#define PIN_SENSOR_7 19 + +#define PIN_SENSOR ( ((PINC<<2) & ~0x03) | ((PINB>>4) & 0x03) ) + +#define PIN_SERVO_HANDLE 10 +#define PIN_SERVO_LANCE 9 + +#define PIN_MOTOR_RIGHT_1 6 +#define PIN_MOTOR_RIGHT_2 8 +#define PIN_MOTOR_LEFT_1 5 +#define PIN_MOTOR_LEFT_2 7 // ex) RIGHT_1:HIGH, RIGHT_2:LOW => 正転 / RIGHT_1:LOW, RIGHT_2:HIGH => 逆転 + +#define STOP 0 +#define GO 1 +#define BACK 2 +#define BRAKE 3 + +#define SPEED_DEFAULT 0xff + +#define HANDLE_DEFAULT 90 //度 +#define HANDLE_MIN ( 800+30) //ms +#define HANDLE_MAX (2300+30) //ms + +#define LANCE_DEFAULT 90 //度 +#define LANCE_MIN 544 //ms +#define LANCE_MAX 2400 //ms + +#define MODE_STOP 0x00 +#define MODE_TRACE 0x10 +#define MODE_RIGHT 0x21 // 右カーブ +#define MODE_LEFT 0x22 // 左カーブ + +#define TARGET_NONE 0x40 +#define TARGET_PARALLEL 0x41 +#define TARGET_VERTICAL 0x42 +#define TARGET_CYLINDER 0x43 + +#define MASK_MODE_TRACE 0b01111110 +#define MASK_CHECK_MARKER 0b10000001 +#define MASK_CHECK_MARKER_RIGHT 0b00000001 +#define MASK_CHECK_MARKER_LEFT 0b10000000 + +/**************************************/ + +#include + +/**************************************/ + +Servo servo_handle; +Servo servo_lance; + +byte mode; +byte target; +unsigned int counter_old; +char handle_angle; +char lance_angle; + +/**************************************/ + +void trace(byte sensor){ + switch( sensor & MASK_MODE_TRACE ){ + case 0b01000000: + handle_angle = -20; + motorMode(GO, GO, 0x40, 0xff); + lance_angle = 0; + break; + case 0b01100000: + handle_angle = -16; + motorMode(GO, GO, 0xff, 0xff); + lance_angle = 0; + break; + case 0b00100000: + handle_angle = -12; + motorMode(GO, GO, 0xff, 0xff); + lance_angle = 0; + break; + case 0b00110000: + handle_angle = -5; + motorMode(GO, GO, 0xff, 0xff); + break; + case 0b00010000: + handle_angle = -2; + motorMode(GO, GO, 0xff, 0xff); + break; + case 0b00011000: + handle_angle = 0; + motorMode(GO, GO, 0xff, 0xff); + break; + case 0b00001000: + handle_angle = 2; + motorMode(GO, GO, 0xff, 0xff); + break; + case 0b00001100: + handle_angle = 5; + motorMode(GO, GO, 0xff, 0xff); + break; + case 0b00000100: + handle_angle = 7; + motorMode(GO, GO, 0xff, 0xff); + break; + case 0b00000110: + handle_angle = 10; + motorMode(GO, GO, 0xff, 0xff); + break; + case 0b00000010: + handle_angle = 12; + motorMode(GO, GO, 0xff, 0xff); + break; + } +} + +void motorMode(byte left_mode, byte right_mode, byte left_speed, byte right_speed){ + analogWrite(PIN_MOTOR_LEFT_1 , left_speed * (left_mode & 0x01) ); + analogWrite(PIN_MOTOR_RIGHT_1, right_speed * (right_mode & 0x01) ); + digitalWrite(PIN_MOTOR_LEFT_2 , left_mode >> 1 ); + digitalWrite(PIN_MOTOR_RIGHT_2, right_mode >> 1 ); +} + +void handle(char handle_angle){ + servo_handle.write(HANDLE_DEFAULT + handle_angle); +} + +void lance(char lance_angle){ + servo_lance.write(LANCE_DEFAULT - lance_angle); +} + +/* ロータリーエンコーダーの変化を見る */ +volatile unsigned int counter = 0; +void count(){ + static byte rot_old = LOW; + static byte rot; + rot = digitalRead(PIN_ROT); + if(rot_old != rot){ + counter++; + rot_old = rot; + } +} + +void setup() { + pinMode(PIN_SENSOR_0, INPUT); + pinMode(PIN_SENSOR_1, INPUT); + pinMode(PIN_SENSOR_2, INPUT); + pinMode(PIN_SENSOR_3, INPUT); + pinMode(PIN_SENSOR_4, INPUT); + pinMode(PIN_SENSOR_5, INPUT); + pinMode(PIN_SENSOR_6, INPUT); + pinMode(PIN_SENSOR_7, INPUT); + + pinMode(PIN_MOTOR_RIGHT_1, OUTPUT); + pinMode(PIN_MOTOR_RIGHT_2, OUTPUT); + pinMode(PIN_MOTOR_LEFT_1 , OUTPUT); + pinMode(PIN_MOTOR_LEFT_2 , OUTPUT); + + pinMode(PIN_ROT, INPUT); + + motorMode(STOP, STOP, 0, 0); + + servo_handle.attach(PIN_SERVO_HANDLE, HANDLE_MIN, HANDLE_MAX); + handle(0); + + servo_lance.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX); + lance(0); + + mode = MODE_STOP; + + digitalWrite(PIN_LED, HIGH); + + delay(1000); + +/**************************************/ + + digitalWrite(PIN_LED, LOW); + mode = MODE_TRACE; + handle(0); + motorMode(GO, GO, 0xff, 0xff); + +} + +/**************************************/ + +void loop(){ + + count(); //ロータリーエンコーダーの変化を見る + + static byte sensor = 0; +#ifdef LINE_BLACK //黒ライン + sensor = PIN_SENSOR; +#else + sensor = ~PIN_SENSOR; +#endif + + switch(target){ + case TARGET_NONE: + lance_angle = LANCE_ANGLE0; + if(counter - counter_old > TARGET_NONE_COUNT ) target = 0; + break; + default: + switch( sensor & MASK_CHECK_MARKER ){ + case 0x81: + target = TARGET_NONE; + counter_old = counter; + break; + case 0x01: + lance_angle = LANCE_ANGLE1; + target = TARGET_PARALLEL; + break; + case 0x80: + lance_angle = LANCE_ANGLE3; + target = TARGET_VERTICAL; + break; + } + break; + } + if( lance_angle == LANCE_ANGLE1 && counter%840 > 420) lance_angle = LANCE_ANGLE4; + static unsigned long millis_lance = 0; + if(millis() - millis_lance > LANCE_INTERVAL){ + millis_lance = millis(); + switch(lance_angle){ + case LANCE_ANGLE1: + lance_angle = LANCE_ANGLE2; + break; + case LANCE_ANGLE2: + lance_angle = LANCE_ANGLE1; + break; + } + } + + switch(mode){ + case MODE_TRACE: + trace(sensor); +#ifdef DEBUG + if( counter > 854*6 - 60 ){ + mode = MODE_STOP; + } +#endif + break; + + case MODE_STOP: + default: + digitalWrite(PIN_LED, HIGH); + handle_angle = 0; + lance_angle = -90; + motorMode(GO, GO, 0xff, 0xff); + if( counter - counter_old > 40 ){ + motorMode(STOP, STOP, 0xff, 0xff); + } + } + + handle(handle_angle); + lance(lance_angle); + +} diff --git a/Arduino/libraries/PSCon/PSCon.cpp b/Arduino/libraries/PSCon/PSCon.cpp new file mode 100644 index 0000000..bbe04ce --- /dev/null +++ b/Arduino/libraries/PSCon/PSCon.cpp @@ -0,0 +1,62 @@ +/* +PSCon.cpp +(c)2011 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +#if defined(ARDUINO) && ARDUINO >= 100 +#include +#else +#include +#endif +#include + +void PSCon::clkPin(byte clkPin){ + pinMode(clkPin, OUTPUT); + PSCon::_clkPin = clkPin; +} + +void PSCon::selPin(byte selPin){ + pinMode(selPin, OUTPUT); + PSCon::_selPin = selPin; +} + +void PSCon::cmdPin(byte cmdPin){ + pinMode(cmdPin, OUTPUT); + PSCon::_cmdPin = cmdPin; +} + +void PSCon::datPin(byte datPin){ + pinMode(datPin, INPUT); + PSCon::_datPin = datPin; +} + +byte PSCon::_getByte(byte requestData){ + for(byte i=0; i<8; i++){ + digitalWrite(PSCon::_clkPin, LOW); + if(requestData & 0x01){ + digitalWrite(PSCon::_cmdPin, HIGH); + } + else{ + digitalWrite(PSCon::_cmdPin, LOW); + } + requestData >>= 1; + delayMicroseconds(1); + digitalWrite(PSCon::_clkPin, HIGH); + if(digitalRead(PSCon::_datPin)){ + requestData |= 0x80; + } + delayMicroseconds(1); + } + digitalWrite(PSCon::_cmdPin, LOW); + delayMicroseconds(8); + return requestData; +} + +void PSCon::get(byte requestData[8]){ + digitalWrite(PSCon::_selPin, LOW); + PSCon::_getByte(0x01); + for(byte i=0; i<8; i++){ + PSCon::data[i] = PSCon::_getByte(requestData[i]); + } + digitalWrite(PSCon::_selPin, HIGH); +} diff --git a/Arduino/libraries/PSCon/PSCon.h b/Arduino/libraries/PSCon/PSCon.h new file mode 100644 index 0000000..25358ee --- /dev/null +++ b/Arduino/libraries/PSCon/PSCon.h @@ -0,0 +1,31 @@ +/* +PSCon.h +(c)2011 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +#ifndef PSCon_h +#define PSCon_h + +#if defined(ARDUINO) && ARDUINO >= 100 +#include +#else +#include +#endif + +class PSCon{ +public: + void clkPin(byte clkPin); + void selPin(byte selPin); + void cmdPin(byte cmdPin); + void datPin(byte datPin); + byte data[8]; + void get(byte requestData[8]); +private: + byte _getByte(byte requestData); + byte _clkPin; + byte _selPin; + byte _cmdPin; + byte _datPin; +}; + +#endif diff --git a/Arduino/libraries/PSCon/examples/PSConSerial/PSConSerial.ino b/Arduino/libraries/PSCon/examples/PSConSerial/PSConSerial.ino new file mode 100644 index 0000000..9b54cc0 --- /dev/null +++ b/Arduino/libraries/PSCon/examples/PSConSerial/PSConSerial.ino @@ -0,0 +1,63 @@ +/* +PSConSerial +(c)2011 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] + +PSCon Arduino +CLK 13 SCK +SEL 10 SS +CMD 11 MOSI +DAT 12 MISO +*/ + +#include + +#define PIN_CLK 13 +#define PIN_SEL 10 +#define PIN_CMD 11 +#define PIN_DAT 12 + +PSCon PSCon; +byte requestData[] = {0x42,0,0,0,0,0,0,0}; + +void setup(){ + Serial.begin(9600); + + PSCon.clkPin(PIN_CLK); + PSCon.selPin(PIN_SEL); + PSCon.cmdPin(PIN_CMD); + PSCon.datPin(PIN_DAT); +} + +void loop(){ + PSCon.get(requestData); + if(PSCon.data[0] == 0x73){ // デジコン:0x41, アナコン:0x73 + Serial.print("R(x,y):("); + Serial.print(PSCon.data[4], DEC); // 左が0x00, 右が0xFF + Serial.print(","); + Serial.print(PSCon.data[5], DEC); // 上が0x00, 下が0xFF + Serial.print(") "); + Serial.print("L(x,y):("); + Serial.print(PSCon.data[6], DEC); // 左が0x00, 右が0xFF + Serial.print(","); + Serial.print(PSCon.data[7], DEC); // 上が0x00, 下が0xFF + Serial.print(") "); + } + if(~PSCon.data[2] & 0x01<<0)Serial.print("sel "); + if(~PSCon.data[2] & 0x01<<1)Serial.print("L3 "); + if(~PSCon.data[2] & 0x01<<2)Serial.print("R3 "); + if(~PSCon.data[2] & 0x01<<3)Serial.print("sta "); + if(~PSCon.data[2] & 0x01<<4)Serial.print("^ "); + if(~PSCon.data[2] & 0x01<<5)Serial.print("> "); + if(~PSCon.data[2] & 0x01<<6)Serial.print("v "); + if(~PSCon.data[2] & 0x01<<7)Serial.print("< "); + if(~PSCon.data[3] & 0x01<<0)Serial.print("L2 "); + if(~PSCon.data[3] & 0x01<<1)Serial.print("R2 "); + if(~PSCon.data[3] & 0x01<<2)Serial.print("L1 "); + if(~PSCon.data[3] & 0x01<<3)Serial.print("R1 "); + if(~PSCon.data[3] & 0x01<<4)Serial.print("% "); + if(~PSCon.data[3] & 0x01<<5)Serial.print("O "); + if(~PSCon.data[3] & 0x01<<6)Serial.print("X "); + if(~PSCon.data[3] & 0x01<<7)Serial.print("# "); + Serial.println(); + delay(16); +} diff --git a/Arduino/libraries/PSCon/keywords.txt b/Arduino/libraries/PSCon/keywords.txt new file mode 100755 index 0000000..def9614 --- /dev/null +++ b/Arduino/libraries/PSCon/keywords.txt @@ -0,0 +1,14 @@ +# for PSCon.h + +# Constants (LITERAL1) + +# Class (KEYWORD1) +PSCon KEYWORD1 + +# Others (KEYWORD2) +clkPin KEYWORD2 +selPin KEYWORD2 +cmdPin KEYWORD2 +datPin KEYWORD2 +data KEYWORD2 +get KEYWORD2 \ No newline at end of file diff --git a/Arduino/libraries/WiiNun/WiiNun.cpp b/Arduino/libraries/WiiNun/WiiNun.cpp new file mode 100644 index 0000000..c16a10b --- /dev/null +++ b/Arduino/libraries/WiiNun/WiiNun.cpp @@ -0,0 +1,37 @@ +/* +WiiNun.cpp +(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +#if defined(ARDUINO) && ARDUINO >= 100 +#include +#else +#include +#endif +#include +#include + +void WiiNun::begin(){ + Wire.begin(); + Wire.beginTransmission(0x52); + Wire.write(0x40); // sends memory address + Wire.write(0x00); + Wire.endTransmission(); +} + +byte WiiNun::_decodeByte(byte x){ + x = (x ^ 0x17) + 0x17; + return x; +} + +void WiiNun::get(){ + byte i=0; + Wire.requestFrom(0x52, 6); + while(Wire.available() || i<6){ + WiiNun::data[i] = WiiNun::_decodeByte(Wire.read()); + i++; + } + Wire.beginTransmission(0x52); + Wire.write(0x00); + Wire.endTransmission(); +} \ No newline at end of file diff --git a/Arduino/libraries/WiiNun/WiiNun.h b/Arduino/libraries/WiiNun/WiiNun.h new file mode 100644 index 0000000..85c22be --- /dev/null +++ b/Arduino/libraries/WiiNun/WiiNun.h @@ -0,0 +1,25 @@ +/* +WiiNun.h +(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] +*/ + +#ifndef WiiNun_h +#define WiiNun_h + +#if defined(ARDUINO) && ARDUINO >= 100 +#include +#else +#include +#endif +#include + +class WiiNun{ + public: + byte data[6]; + void begin(); + void get(); + private: + byte _decodeByte(byte x); +}; + +#endif \ No newline at end of file diff --git a/Arduino/libraries/WiiNun/examples/WiiNunSerial/WiiNunSerial.ino b/Arduino/libraries/WiiNun/examples/WiiNunSerial/WiiNunSerial.ino new file mode 100644 index 0000000..a4f3ef8 --- /dev/null +++ b/Arduino/libraries/WiiNun/examples/WiiNunSerial/WiiNunSerial.ino @@ -0,0 +1,59 @@ +/* +WiiNunSerial +(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] + +WiiNun <= I2C => Arduino +CLK A5 CLK +DAT A4 DAT +PWR A3 +GND A2 +*/ + +#define PIN_PWR A3 +#define PIN_GND A2 + +#include +#include + +WiiNun WiiNun; + +void setup(){ + pinMode(PIN_PWR, OUTPUT); + pinMode(PIN_GND, OUTPUT); + digitalWrite(PIN_PWR, HIGH); + digitalWrite(PIN_GND, LOW); + delay(100); + Serial.begin(9600); + WiiNun.begin(); +} + +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]; + 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; + Serial.print("Joy(x,y):("); + Serial.print(joyx, DEC); // 0x00:左, 0xFF:右 + Serial.print(","); + Serial.print(joyy, DEC); // 0x00:下, 0xFF:上 + Serial.print(") "); + Serial.print("Acc(x,y,z):("); + Serial.print(accx, DEC); // + Serial.print(","); + Serial.print(accy, DEC); // + Serial.print(","); + Serial.print(accz, DEC); // + Serial.print(") "); + if(~WiiNun.data[5] & 0x01<<0)Serial.print("Z "); + if(~WiiNun.data[5] & 0x01<<1)Serial.print("C "); + Serial.println(); + delay(16); +} \ No newline at end of file diff --git a/Arduino/libraries/WiiNun/keywords.txt b/Arduino/libraries/WiiNun/keywords.txt new file mode 100755 index 0000000..a090a32 --- /dev/null +++ b/Arduino/libraries/WiiNun/keywords.txt @@ -0,0 +1,11 @@ +# for WiiNun.h + +# Constants (LITERAL1) + +# Class (KEYWORD1) +WiiNun KEYWORD1 + +# Others (KEYWORD2) +data KEYWORD2 +begin KEYWORD2 +get KEYWORD2 \ No newline at end of file diff --git a/Arduino/mini_rhinoceros/mini_rhinoceros.pde b/Arduino/mini_rhinoceros/mini_rhinoceros.pde new file mode 100644 index 0000000..e504252 --- /dev/null +++ b/Arduino/mini_rhinoceros/mini_rhinoceros.pde @@ -0,0 +1,40 @@ +#define pin_l_0 6 +#define pin_l_1 7 +#define pin_r_1 8 +#define pin_r_0 9 +#define pin_sensor_0 3 +#define pin_sensor_1 2 + +byte sensor; + +void setup(){ + + pinMode(pin_l_0,OUTPUT); + pinMode(pin_l_1,OUTPUT); + + pinMode(pin_r_0,OUTPUT); + pinMode(pin_r_1,OUTPUT); + + digitalWrite(pin_l_0,HIGH); + digitalWrite(pin_l_1,LOW); + digitalWrite(pin_r_0,HIGH); + digitalWrite(pin_r_1,LOW); + + /*--------------------*/ + + sensor = 0b11; + +} + +void loop(){ + + if(analogRead(pin_sensor_0)<100){ + sensor = 0b10; + }else if(analogRead(pin_sensor_1)<100){ + sensor = 0b01; + } + + digitalWrite(pin_r_0,(sensor & 0b01) >> 0 ); + digitalWrite(pin_l_0,(sensor & 0b10) >> 1 ); + +}