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-29 16:16:33 +09:00
parent a9ba272657
commit 4c663f510a
1 changed files with 36 additions and 37 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

71
arduino/hanzo2_1/hanzo2_1.ino
View file

@ -35,8 +35,8 @@ const unsigned int HANDLE_MIN = ( 800+30); //us
const unsigned int HANDLE_MAX = (2300+30); //us
const int LANCE_DEGREE_DEFAULT = 90; //度
const unsigned int LANCE_MIN = 544; //us
const unsigned int LANCE_MAX = 2400; //us
const unsigned int LANCE_MIN = ( 544-62); //us
const unsigned int LANCE_MAX = (2400-62); //us
const byte PIN_LED = 4;
const byte PIN_ROT = 2;
@ -82,6 +82,7 @@ Motor motorR;
Motor motorL;
byte mode;
volatile unsigned long counter = 0;
unsigned long counterOld;
/**************************************/
@ -90,18 +91,15 @@ void trace(byte sensor){
switch(sensor&MASK_MODE_TRACE){
case 0b01000000:
handle(-20);
motorMode(GO, GO, SPEED_DEFAULT/4, SPEED_DEFAULT);
// lance(0);
motorMode(GO, GO, 0, SPEED_DEFAULT);
break;
case 0b01100000:
handle(-16);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
// lance(0);
break;
case 0b00100000:
handle(-12);
motorMode(GO, GO, SPEED_DEFAULT, SPEED_DEFAULT);
// lance(0);
break;
case 0b00110000:
handle(-5);
@ -138,6 +136,31 @@ void trace(byte sensor){
}
}
void motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){
motorL.mode( leftMode, leftSpeed);
motorR.mode(rightMode, rightSpeed);
}
void handle(int degree){
servoHandle.write(HANDLE_DEGREE_DEFAULT + degree);
}
void lance(int degree){
servoLance.write(LANCE_DEGREE_DEFAULT - degree);
}
/* ロータリーエンコーダーの変化を見る
todo:外部割り込み使ってみたい、、、
*/
void count(){
static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態
rot[0] = digitalRead(PIN_ROT);
if(rot[0] != rot[1]){
rot[1] = rot[0];
counter++;
}
}
int checkMarker(byte sensor){
int mode = MODE_TRACE;
switch(sensor & MASK_CHECK_MARKER){
@ -159,32 +182,6 @@ int checkMarker(byte sensor){
return mode;
}
void motorMode(byte leftMode, byte rightMode, byte leftSpeed, byte rightSpeed){
motorL.mode( leftMode, leftSpeed);
motorR.mode(rightMode, rightSpeed);
}
void handle(int degree){
servoHandle.write(HANDLE_DEGREE_DEFAULT + degree);
}
void lance(int degree){
servoLance.write(LANCE_DEGREE_DEFAULT - degree);
}
/* ロータリーエンコーダーの変化を見る
todo:外部割り込み使ってみたい、、、
*/
volatile unsigned long counter = 0;
void count(){
static byte rot[2] = {0}; //rot[0]:今の状態, rot[1]:前回の状態
rot[0] = digitalRead(PIN_ROT);
if(rot[0] != rot[1]){
rot[1] = rot[0];
counter++;
}
}
void sensorInit(){
pinMode(PIN_SENSOR_0, INPUT);
pinMode(PIN_SENSOR_1, INPUT);
@ -268,8 +265,10 @@ void loop(){
trace(sensor);
mode = checkMarker(sensor);
// 6周する前に、円筒標的を狙いに行く
if( counter > LAP_COUNT*6 - LAP_COUNT/14 ){
mode = MODE_TARGET_CYLINDER;
// if( counter > LAP_COUNT*6 - LAP_COUNT/14 ){
// mode = MODE_TARGET_CYLINDER;
if(counter > LAP_COUNT){
mode = MODE_STOP;
}
break;
case MODE_LEFT: //左カーブ
@ -298,10 +297,10 @@ void loop(){
if(millis() - millis_lance > LANCE_INTERVAL){
millis_lance = millis();
switch(servoLance.read()){
case LANCE_ANGLE1:
case LANCE_DEGREE_DEFAULT - LANCE_ANGLE1:
lance(LANCE_ANGLE2);
break;
case LANCE_ANGLE2:
case LANCE_DEGREE_DEFAULT - LANCE_ANGLE2:
lance(LANCE_ANGLE1);
break;
}