同期
This commit is contained in:
parent
534e82f706
commit
1017975df7
20 changed files with 1519 additions and 0 deletions
18
Arduino/AnalogReadSerial_x6/AnalogReadSerial_x6.ino
Normal file
18
Arduino/AnalogReadSerial_x6/AnalogReadSerial_x6.ino
Normal file
|
|
@ -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');
|
||||
}
|
||||
12
Arduino/Default/Default.ino
Normal file
12
Arduino/Default/Default.ino
Normal file
|
|
@ -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(){
|
||||
|
||||
}
|
||||
18
Arduino/DigitalReadSerial_x6/DigitalReadSerial_x6.ino
Normal file
18
Arduino/DigitalReadSerial_x6/DigitalReadSerial_x6.ino
Normal file
|
|
@ -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');
|
||||
}
|
||||
30
Arduino/DigitalReadSerial_x8/DigitalReadSerial_x8.ino
Normal file
30
Arduino/DigitalReadSerial_x8/DigitalReadSerial_x8.ino
Normal file
|
|
@ -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');
|
||||
}
|
||||
64
Arduino/PSConSPI/PSConSPI.ino
Normal file
64
Arduino/PSConSPI/PSConSPI.ino
Normal file
|
|
@ -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 <SPI.h>
|
||||
|
||||
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);
|
||||
}
|
||||
33
Arduino/ServoSet/ServoSet.ino
Normal file
33
Arduino/ServoSet/ServoSet.ino
Normal file
|
|
@ -0,0 +1,33 @@
|
|||
#include <Servo.h>
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
33
Arduino/ServoSetMicroseconds/ServoSetMicroseconds.ino
Normal file
33
Arduino/ServoSetMicroseconds/ServoSetMicroseconds.ino
Normal file
|
|
@ -0,0 +1,33 @@
|
|||
#include <Servo.h>
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
181
Arduino/catchrobo/catchrobo.ino
Executable file
181
Arduino/catchrobo/catchrobo.ino
Executable file
|
|
@ -0,0 +1,181 @@
|
|||
#include <Servo.h>
|
||||
#include <Wire.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 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);
|
||||
}
|
||||
132
Arduino/catchrobo2012/catchrobo2012 .ino
Normal file
132
Arduino/catchrobo2012/catchrobo2012 .ino
Normal file
|
|
@ -0,0 +1,132 @@
|
|||
#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);
|
||||
}
|
||||
364
Arduino/hanzo1/hanzo1.pde
Normal file
364
Arduino/hanzo1/hanzo1.pde
Normal file
|
|
@ -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 <Servo.h>
|
||||
// #include <MsTimer2.h>
|
||||
|
||||
/**************************************/
|
||||
|
||||
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
|
||||
}
|
||||
|
||||
292
Arduino/hanzo2/hanzo2.ino
Normal file
292
Arduino/hanzo2/hanzo2.ino
Normal file
|
|
@ -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.h>
|
||||
|
||||
/**************************************/
|
||||
|
||||
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);
|
||||
|
||||
}
|
||||
62
Arduino/libraries/PSCon/PSCon.cpp
Normal file
62
Arduino/libraries/PSCon/PSCon.cpp
Normal file
|
|
@ -0,0 +1,62 @@
|
|||
/*
|
||||
PSCon.cpp
|
||||
(c)2011 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
||||
*/
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include <Arduino.h>
|
||||
#else
|
||||
#include <WProgram.h>
|
||||
#endif
|
||||
#include <PSCon.h>
|
||||
|
||||
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);
|
||||
}
|
||||
31
Arduino/libraries/PSCon/PSCon.h
Normal file
31
Arduino/libraries/PSCon/PSCon.h
Normal file
|
|
@ -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 <Arduino.h>
|
||||
#else
|
||||
#include <WProgram.h>
|
||||
#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
|
||||
63
Arduino/libraries/PSCon/examples/PSConSerial/PSConSerial.ino
Normal file
63
Arduino/libraries/PSCon/examples/PSConSerial/PSConSerial.ino
Normal file
|
|
@ -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 <PSCon.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
14
Arduino/libraries/PSCon/keywords.txt
Executable file
14
Arduino/libraries/PSCon/keywords.txt
Executable file
|
|
@ -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
|
||||
37
Arduino/libraries/WiiNun/WiiNun.cpp
Normal file
37
Arduino/libraries/WiiNun/WiiNun.cpp
Normal file
|
|
@ -0,0 +1,37 @@
|
|||
/*
|
||||
WiiNun.cpp
|
||||
(c)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
||||
*/
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include <Arduino.h>
|
||||
#else
|
||||
#include <WProgram.h>
|
||||
#endif
|
||||
#include <Wire.h>
|
||||
#include <WiiNun.h>
|
||||
|
||||
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();
|
||||
}
|
||||
25
Arduino/libraries/WiiNun/WiiNun.h
Normal file
25
Arduino/libraries/WiiNun/WiiNun.h
Normal file
|
|
@ -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 <Arduino.h>
|
||||
#else
|
||||
#include <WProgram.h>
|
||||
#endif
|
||||
#include <Wire.h>
|
||||
|
||||
class WiiNun{
|
||||
public:
|
||||
byte data[6];
|
||||
void begin();
|
||||
void get();
|
||||
private:
|
||||
byte _decodeByte(byte x);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -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 <Wire.h>
|
||||
#include <WiiNun.h>
|
||||
|
||||
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);
|
||||
}
|
||||
11
Arduino/libraries/WiiNun/keywords.txt
Executable file
11
Arduino/libraries/WiiNun/keywords.txt
Executable file
|
|
@ -0,0 +1,11 @@
|
|||
# for WiiNun.h
|
||||
|
||||
# Constants (LITERAL1)
|
||||
|
||||
# Class (KEYWORD1)
|
||||
WiiNun KEYWORD1
|
||||
|
||||
# Others (KEYWORD2)
|
||||
data KEYWORD2
|
||||
begin KEYWORD2
|
||||
get KEYWORD2
|
||||
40
Arduino/mini_rhinoceros/mini_rhinoceros.pde
Normal file
40
Arduino/mini_rhinoceros/mini_rhinoceros.pde
Normal file
|
|
@ -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 );
|
||||
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue