nebel/2014-git-work
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

update

Browse source
This commit is contained in:
Nebel 2012-09-14 13:52:25 +09:00
parent 2a11d55613
commit 784b38eeb2
1 changed files with 76 additions and 54 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

130
arduino/hanzo2_1/hanzo2_1.ino
View file

@ -19,11 +19,11 @@
#include <Servo.h> #include <Servo.h>
#include "Motor.h" #include "Motor.h"
/**************************************/
//#define LINE_BLACK //白地・黒ライン有効 //#define LINE_BLACK //白地・黒ライン有効
#define PIN_SENSOR ( ((PINC<<2) & ~0x03) | ((PINB>>4) & 0x03) ) #define PIN_SENSOR ( ((PINC<<2) & ~0x03) | ((PINB>>4) & 0x03) )
const unsigned int TARGET_NONE_COUNT = 85; const unsigned int TARGET_NONE_COUNT = 85;
const unsigned long LAP_COUNT = 854;
const int LANCE_ANGLE0 = 0; //度 const int LANCE_ANGLE0 = 0; //度
const int LANCE_ANGLE1 = 43; const int LANCE_ANGLE1 = 43;
@ -34,11 +34,11 @@ const unsigned int LANCE_INTERVAL = 70; //ms
const byte SPEED_DEFAULT = 0xff; const byte SPEED_DEFAULT = 0xff;
const int HANDLE_DEFAULT = 90; //度 const int HANDLE_DEGREE_DEFAULT = 90; //度
const unsigned int HANDLE_MIN = ( 800+30); //ms const unsigned int HANDLE_MIN = ( 800+30); //ms
const unsigned int HANDLE_MAX = (2300+30); //ms const unsigned int HANDLE_MAX = (2300+30); //ms
const int LANCE_DEFAULT = 90; //度 const int LANCE_DEGREE_DEFAULT = 90; //度
const unsigned int LANCE_MIN = 544; //ms const unsigned int LANCE_MIN = 544; //ms
const unsigned int LANCE_MAX = 2400; //ms const unsigned int LANCE_MAX = 2400; //ms
@ -58,19 +58,19 @@ const byte PIN_SERVO_HANDLE = 10;
const byte PIN_SERVO_LANCE = 9; const byte PIN_SERVO_LANCE = 9;
const byte PIN_MOTOR_LEFT_1 = 5; const byte PIN_MOTOR_LEFT_1 = 5;
const byte PIN_MOTOR_LEFT_2 = 4; const byte PIN_MOTOR_LEFT_2 = 7;
const byte PIN_MOTOR_RIGHT_1 = 6; const byte PIN_MOTOR_RIGHT_1 = 6;
const byte PIN_MOTOR_RIGHT_2 = 7; const byte PIN_MOTOR_RIGHT_2 = 8;
const byte MODE_STOP = 0; const byte MODE_STOP = 0x10;
const byte MODE_TRACE = 10; const byte MODE_TRACE = 0x11;
const byte MODE_RIGHT = 21; // 右カーブ const byte MODE_LEFT = 0x12; //左カーブ
const byte MODE_LEFT = 22; // 左カーブ const byte MODE_RIGHT = 0x13; //右カーブ
const byte TARGET_NONE = 40; const byte TARGET_NONE = 0x40;
const byte TARGET_PARALLEL = 41; const byte TARGET_PARALLEL = 0x41; //平行標的
const byte TARGET_VERTICAL = 42; const byte TARGET_VERTICAL = 0x42; //垂直標的
const byte TARGET_CYLINDER = 43; const byte TARGET_CYLINDER = 0x43; //円筒標的
const byte MASK_MODE_TRACE = 0b01111110; const byte MASK_MODE_TRACE = 0b01111110;
const byte MASK_CHECK_MARKER = 0b10000001; const byte MASK_CHECK_MARKER = 0b10000001;
@ -87,7 +87,6 @@ Motor motorL;
byte mode; byte mode;
byte target; byte target;
unsigned int counterOld; unsigned int counterOld;
char handleAngle;
char lanceAngle; char lanceAngle;
/**************************************/ /**************************************/
@ -95,78 +94,76 @@ char lanceAngle;
void trace(byte sensor){ void trace(byte sensor){
switch( sensor & MASK_MODE_TRACE ){ switch( sensor & MASK_MODE_TRACE ){
case 0b01000000: case 0b01000000:
handleAngle = -20; handle(-20);
motorMode(GO, GO, 0x40, 0xff); motorMode(GO, GO, 0x40, 0xff);
lanceAngle = 0; lanceAngle = 0;
break; break;
case 0b01100000: case 0b01100000:
handleAngle = -16; handle(-16);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
lanceAngle = 0; lanceAngle = 0;
break; break;
case 0b00100000: case 0b00100000:
handleAngle = -12; handle(-12);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
lanceAngle = 0; lanceAngle = 0;
break; break;
case 0b00110000: case 0b00110000:
handleAngle = -5; handle(-5);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
break; break;
case 0b00010000: case 0b00010000:
handleAngle = -2; handle(-2);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
break; break;
case 0b00011000: case 0b00011000:
handleAngle = 0; handle(0);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
break; break;
case 0b00001000: case 0b00001000:
handleAngle = 2; handle(2);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
break; break;
case 0b00001100: case 0b00001100:
handleAngle = 5; handle(5);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
break; break;
case 0b00000100: case 0b00000100:
handleAngle = 7; handle(7);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
break; break;
case 0b00000110: case 0b00000110:
handleAngle = 10; handle(10);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
break; break;
case 0b00000010: case 0b00000010:
handleAngle = 12; handle(12);
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
break; break;
} }
} }
/**************************************/
void motorMode(byte motorLeftMode, byte motorRightMode, byte motorLeftSpeed, byte motorRightSpeed){ void motorMode(byte motorLeftMode, byte motorRightMode, byte motorLeftSpeed, byte motorRightSpeed){
motorL.mode( motorLeftMode, motorLeftSpeed); motorL.mode( motorLeftMode, motorLeftSpeed);
motorR.mode(motorRightMode, motorRightSpeed); motorR.mode(motorRightMode, motorRightSpeed);
} }
void handle(char handle_angle){ void handle(int degree){
servoHandle.write(HANDLE_DEFAULT + handle_angle); servoHandle.write(HANDLE_DEGREE_DEFAULT + degree);
} }
void lance(char lance_angle){ void lance(int degree){
servoLance.write(LANCE_DEFAULT - lance_angle); servoLance.write(LANCE_DEGREE_DEFAULT - degree);
} }
/* ロータリーエンコーダーの変化を見る */ /* ロータリーエンコーダーの変化を見る */
volatile unsigned long counter = 0; volatile unsigned long counter = 0;
void count(){ void count(){
static byte rot[2] = {0}; static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態
rot[1] = digitalRead(PIN_ROT); rot[0] = digitalRead(PIN_ROT);
if(rot[1] != rot[0]){ if(rot[0] != rot[1]){
rot[1] = rot[0];
counter++; counter++;
rot[0] = rot[1];
} }
} }
@ -226,23 +223,23 @@ void loop(){
if(counter - counterOld > TARGET_NONE_COUNT ) target = 0; if(counter - counterOld > TARGET_NONE_COUNT ) target = 0;
break; break;
default: default:
switch( sensor & MASK_CHECK_MARKER ){ switch( sensor & MASK_CHECK_MARKER ){
case 0x81: case 0x81:
target = TARGET_NONE; target = TARGET_NONE;
counterOld = counter; counterOld = counter;
break; break;
case 0x01: case 0x01:
lanceAngle = LANCE_ANGLE1; lanceAngle = LANCE_ANGLE1;
target = TARGET_PARALLEL; target = TARGET_PARALLEL;
break; break;
case 0x80: case 0x80:
lanceAngle = LANCE_ANGLE3; lanceAngle = LANCE_ANGLE3;
target = TARGET_VERTICAL; target = TARGET_VERTICAL;
break;
}
break; break;
} }
if( lanceAngle == LANCE_ANGLE1 && counter%840 > 420) lanceAngle = LANCE_ANGLE4; break;
}
if( lanceAngle == LANCE_ANGLE1 && counter%LAP_COUNT > LAP_COUNT/2) lanceAngle = LANCE_ANGLE4;
static unsigned long millis_lance = 0; static unsigned long millis_lance = 0;
if(millis() - millis_lance > LANCE_INTERVAL){ if(millis() - millis_lance > LANCE_INTERVAL){
millis_lance = millis(); millis_lance = millis();
@ -255,25 +252,50 @@ void loop(){
break; break;
} }
} }
//ランスの動作を決定する
switch(target){
case TARGET_NONE:
lanceAngle = LANCE_ANGLE0;
if(counter - counterOld > TARGET_NONE_COUNT ) target = 0;
break;
case TARGET_PARALLEL:
case TARGET_VERTICAL:
case TARGET_CYLINDER:
default:
switch( sensor & MASK_CHECK_MARKER ){
case 0x81:
target = TARGET_NONE;
counterOld = counter;
break;
case 0x01:
lanceAngle = LANCE_ANGLE1;
target = TARGET_PARALLEL;
break;
case 0x80:
lanceAngle = LANCE_ANGLE3;
target = TARGET_VERTICAL;
break;
}
break;
}
//全体の動作を決定する //全体の動作を決定する
switch(mode){ switch(mode){
case MODE_TRACE: case MODE_TRACE:
trace(sensor); trace(sensor);
if( counter > 854*6 - 60 ){ // 6周(1周854カウント)したら、円筒標的を狙いに行く if( counter > LAP_COUNT*6 - LAP_COUNT/14 ){ // 6周したら、円筒標的を狙いに行く
mode = MODE_STOP; mode = MODE_STOP;
} }
break; break;
case MODE_STOP: case MODE_STOP:
default: default:
digitalWrite(PIN_LED, HIGH); digitalWrite(PIN_LED, HIGH);
handleAngle = 0; handle(0);
lanceAngle = -90; lanceAngle = -90;
motorMode(GO, GO, 0xff, 0xff); motorMode(GO, GO, 0xff, 0xff);
if( counter - counterOld > 40 ){ if( counter - counterOld > LAP_COUNT/21 ){
motorMode(STOP, STOP, 0xff, 0xff); motorMode(STOP, STOP, 0xff, 0xff);
} }
} }
handle(handleAngle);
lance(lanceAngle); lance(lanceAngle);
} }