arduinoに変更
This commit is contained in:
parent
274290aa8f
commit
a7e3bc6799
20 changed files with 0 additions and 1519 deletions
|
|
@ -1,18 +0,0 @@
|
||||||
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');
|
|
||||||
}
|
|
||||||
|
|
@ -1,12 +0,0 @@
|
||||||
// (C)2012 kou029w - MIT License
|
|
||||||
|
|
||||||
int analogRead(uint8_t pin, uint8_t max){
|
|
||||||
return ((long)analogRead(pin)*max)>>10;
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup(){
|
|
||||||
|
|
||||||
}
|
|
||||||
void loop(){
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
@ -1,18 +0,0 @@
|
||||||
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');
|
|
||||||
}
|
|
||||||
|
|
@ -1,30 +0,0 @@
|
||||||
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');
|
|
||||||
}
|
|
||||||
|
|
@ -1,64 +0,0 @@
|
||||||
/*
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
@ -1,33 +0,0 @@
|
||||||
#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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
@ -1,33 +0,0 @@
|
||||||
#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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
@ -1,181 +0,0 @@
|
||||||
#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);
|
|
||||||
}
|
|
||||||
|
|
@ -1,132 +0,0 @@
|
||||||
#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);
|
|
||||||
}
|
|
||||||
|
|
@ -1,364 +0,0 @@
|
||||||
/*
|
|
||||||
競技用ランサーロボット
|
|
||||||
半蔵 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
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
@ -1,292 +0,0 @@
|
||||||
/*
|
|
||||||
競技用ランサーロボット
|
|
||||||
半蔵 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);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
@ -1,62 +0,0 @@
|
||||||
/*
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
@ -1,31 +0,0 @@
|
||||||
/*
|
|
||||||
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
|
|
||||||
|
|
@ -1,63 +0,0 @@
|
||||||
/*
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
@ -1,14 +0,0 @@
|
||||||
# for PSCon.h
|
|
||||||
|
|
||||||
# Constants (LITERAL1)
|
|
||||||
|
|
||||||
# Class (KEYWORD1)
|
|
||||||
PSCon KEYWORD1
|
|
||||||
|
|
||||||
# Others (KEYWORD2)
|
|
||||||
clkPin KEYWORD2
|
|
||||||
selPin KEYWORD2
|
|
||||||
cmdPin KEYWORD2
|
|
||||||
datPin KEYWORD2
|
|
||||||
data KEYWORD2
|
|
||||||
get KEYWORD2
|
|
||||||
|
|
@ -1,37 +0,0 @@
|
||||||
/*
|
|
||||||
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();
|
|
||||||
}
|
|
||||||
|
|
@ -1,25 +0,0 @@
|
||||||
/*
|
|
||||||
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
|
|
||||||
|
|
@ -1,59 +0,0 @@
|
||||||
/*
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
@ -1,11 +0,0 @@
|
||||||
# for WiiNun.h
|
|
||||||
|
|
||||||
# Constants (LITERAL1)
|
|
||||||
|
|
||||||
# Class (KEYWORD1)
|
|
||||||
WiiNun KEYWORD1
|
|
||||||
|
|
||||||
# Others (KEYWORD2)
|
|
||||||
data KEYWORD2
|
|
||||||
begin KEYWORD2
|
|
||||||
get KEYWORD2
|
|
||||||
|
|
@ -1,40 +0,0 @@
|
||||||
#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