/* 競技用ランサーロボット 半蔵 2.0 (C)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt] */ /* <前> [7 654321 0] | | []---*----[] | / |/ []---#----[] */ //#define LINE_BLACK //白地・黒ライン有効 #define TARGET_NONE_COUNT 85 #define LANCE_ANGLE0 0 //度 #define LANCE_ANGLE1 43 #define LANCE_ANGLE2 54 #define LANCE_ANGLE3 -33 #define LANCE_ANGLE4 34 #define LANCE_INTERVAL 70 //ms #define PIN_LED 4 #define PIN_ROT 2 #define PIN_SENSOR_0 12 #define PIN_SENSOR_1 13 #define PIN_SENSOR_2 14 #define PIN_SENSOR_3 15 #define PIN_SENSOR_4 16 #define PIN_SENSOR_5 17 #define PIN_SENSOR_6 18 #define PIN_SENSOR_7 19 #define PIN_SENSOR ( ((PINC<<2) & ~0x03) | ((PINB>>4) & 0x03) ) #define PIN_SERVO_HANDLE 10 #define PIN_SERVO_LANCE 9 #define PIN_MOTOR_RIGHT_1 6 #define PIN_MOTOR_RIGHT_2 8 #define PIN_MOTOR_LEFT_1 5 #define PIN_MOTOR_LEFT_2 7 // ex) RIGHT_1:HIGH, RIGHT_2:LOW => 正転 / RIGHT_1:LOW, RIGHT_2:HIGH => 逆転 #define STOP 0 #define GO 1 #define BACK 2 #define BRAKE 3 #define SPEED_DEFAULT 0xff #define HANDLE_DEFAULT 90 //度 #define HANDLE_MIN ( 800+30) //ms #define HANDLE_MAX (2300+30) //ms #define LANCE_DEFAULT 90 //度 #define LANCE_MIN 544 //ms #define LANCE_MAX 2400 //ms #define MODE_STOP 0x00 #define MODE_TRACE 0x10 #define MODE_RIGHT 0x21 // 右カーブ #define MODE_LEFT 0x22 // 左カーブ #define TARGET_NONE 0x40 #define TARGET_PARALLEL 0x41 #define TARGET_VERTICAL 0x42 #define TARGET_CYLINDER 0x43 #define MASK_MODE_TRACE 0b01111110 #define MASK_CHECK_MARKER 0b10000001 #define MASK_CHECK_MARKER_RIGHT 0b00000001 #define MASK_CHECK_MARKER_LEFT 0b10000000 /**************************************/ #include /**************************************/ Servo servo_handle; Servo servo_lance; byte mode; byte target; unsigned int counter_old; char handle_angle; char lance_angle; /**************************************/ void trace(byte sensor){ switch( sensor & MASK_MODE_TRACE ){ case 0b01000000: handle_angle = -20; motorMode(GO, GO, 0x40, 0xff); lance_angle = 0; break; case 0b01100000: handle_angle = -16; motorMode(GO, GO, 0xff, 0xff); lance_angle = 0; break; case 0b00100000: handle_angle = -12; motorMode(GO, GO, 0xff, 0xff); lance_angle = 0; break; case 0b00110000: handle_angle = -5; motorMode(GO, GO, 0xff, 0xff); break; case 0b00010000: handle_angle = -2; motorMode(GO, GO, 0xff, 0xff); break; case 0b00011000: handle_angle = 0; motorMode(GO, GO, 0xff, 0xff); break; case 0b00001000: handle_angle = 2; motorMode(GO, GO, 0xff, 0xff); break; case 0b00001100: handle_angle = 5; motorMode(GO, GO, 0xff, 0xff); break; case 0b00000100: handle_angle = 7; motorMode(GO, GO, 0xff, 0xff); break; case 0b00000110: handle_angle = 10; motorMode(GO, GO, 0xff, 0xff); break; case 0b00000010: handle_angle = 12; motorMode(GO, GO, 0xff, 0xff); break; } } void motorMode(byte left_mode, byte right_mode, byte left_speed, byte right_speed){ analogWrite(PIN_MOTOR_LEFT_1 , left_speed * (left_mode & 0x01) ); analogWrite(PIN_MOTOR_RIGHT_1, right_speed * (right_mode & 0x01) ); digitalWrite(PIN_MOTOR_LEFT_2 , left_mode >> 1 ); digitalWrite(PIN_MOTOR_RIGHT_2, right_mode >> 1 ); } void handle(char handle_angle){ servo_handle.write(HANDLE_DEFAULT + handle_angle); } void lance(char lance_angle){ servo_lance.write(LANCE_DEFAULT - lance_angle); } /* ロータリーエンコーダーの変化を見る */ volatile unsigned int counter = 0; void count(){ static byte rot_old = LOW; static byte rot; rot = digitalRead(PIN_ROT); if(rot_old != rot){ counter++; rot_old = rot; } } void setup() { pinMode(PIN_SENSOR_0, INPUT); pinMode(PIN_SENSOR_1, INPUT); pinMode(PIN_SENSOR_2, INPUT); pinMode(PIN_SENSOR_3, INPUT); pinMode(PIN_SENSOR_4, INPUT); pinMode(PIN_SENSOR_5, INPUT); pinMode(PIN_SENSOR_6, INPUT); pinMode(PIN_SENSOR_7, INPUT); pinMode(PIN_MOTOR_RIGHT_1, OUTPUT); pinMode(PIN_MOTOR_RIGHT_2, OUTPUT); pinMode(PIN_MOTOR_LEFT_1 , OUTPUT); pinMode(PIN_MOTOR_LEFT_2 , OUTPUT); pinMode(PIN_ROT, INPUT); motorMode(STOP, STOP, 0, 0); servo_handle.attach(PIN_SERVO_HANDLE, HANDLE_MIN, HANDLE_MAX); handle(0); servo_lance.attach(PIN_SERVO_LANCE, LANCE_MIN, LANCE_MAX); lance(0); mode = MODE_STOP; digitalWrite(PIN_LED, HIGH); delay(1000); /**************************************/ digitalWrite(PIN_LED, LOW); mode = MODE_TRACE; handle(0); motorMode(GO, GO, 0xff, 0xff); } /**************************************/ void loop(){ count(); //ロータリーエンコーダーの変化を見る static byte sensor = 0; #ifdef LINE_BLACK //黒ライン sensor = PIN_SENSOR; #else sensor = ~PIN_SENSOR; #endif switch(target){ case TARGET_NONE: lance_angle = LANCE_ANGLE0; if(counter - counter_old > TARGET_NONE_COUNT ) target = 0; break; default: switch( sensor & MASK_CHECK_MARKER ){ case 0x81: target = TARGET_NONE; counter_old = counter; break; case 0x01: lance_angle = LANCE_ANGLE1; target = TARGET_PARALLEL; break; case 0x80: lance_angle = LANCE_ANGLE3; target = TARGET_VERTICAL; break; } break; } if( lance_angle == LANCE_ANGLE1 && counter%840 > 420) lance_angle = LANCE_ANGLE4; static unsigned long millis_lance = 0; if(millis() - millis_lance > LANCE_INTERVAL){ millis_lance = millis(); switch(lance_angle){ case LANCE_ANGLE1: lance_angle = LANCE_ANGLE2; break; case LANCE_ANGLE2: lance_angle = LANCE_ANGLE1; break; } } switch(mode){ case MODE_TRACE: trace(sensor); if( counter > 854*6 - 60 ){ // 6周(1周854カウント)したら、円筒標的を狙いに行く mode = MODE_STOP; } 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); }