Junichi Katsu
ライントレーサーのテストプログラムです。 コンパイルのみ 動作未チェック
main.cpp@0:72366f00d54c, 2012-04-01 (annotated)
- Committer:
- jksoft
- Date:
- Sun Apr 01 06:48:54 2012 +0000
- Revision:
- 0:72366f00d54c
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jksoft | 0:72366f00d54c | 1 | // Line Trace Test Program |
| jksoft | 0:72366f00d54c | 2 | // By JKSOFT |
| jksoft | 0:72366f00d54c | 3 | |
| jksoft | 0:72366f00d54c | 4 | // Line Sensor Port |
| jksoft | 0:72366f00d54c | 5 | // Left Center Right |
| jksoft | 0:72366f00d54c | 6 | // p16 p17 p18 p19 p20 |
| jksoft | 0:72366f00d54c | 7 | // 0x01 0x02 0x04 0x08 0x10 |
| jksoft | 0:72366f00d54c | 8 | |
| jksoft | 0:72366f00d54c | 9 | // Motor Port |
| jksoft | 0:72366f00d54c | 10 | // Left motor1(M1) p5,p6 Forward = 0x01 Back = 0x02 |
| jksoft | 0:72366f00d54c | 11 | // Right motor2(M2) p7,p8 Forward = 0x01 Back = 0x02 |
| jksoft | 0:72366f00d54c | 12 | |
| jksoft | 0:72366f00d54c | 13 | // Servo Port |
| jksoft | 0:72366f00d54c | 14 | // p21 |
| jksoft | 0:72366f00d54c | 15 | // Center 0.5 |
| jksoft | 0:72366f00d54c | 16 | // Left Max 0.0 |
| jksoft | 0:72366f00d54c | 17 | // Right Max 1.0 |
| jksoft | 0:72366f00d54c | 18 | |
| jksoft | 0:72366f00d54c | 19 | #include "mbed.h" |
| jksoft | 0:72366f00d54c | 20 | #include "Servo.h" |
| jksoft | 0:72366f00d54c | 21 | |
| jksoft | 0:72366f00d54c | 22 | #define SENSOR_TH 0x8000 |
| jksoft | 0:72366f00d54c | 23 | #define SENSOR_NUM 5 |
| jksoft | 0:72366f00d54c | 24 | |
| jksoft | 0:72366f00d54c | 25 | #define SERVO_CENTER 0.5 |
| jksoft | 0:72366f00d54c | 26 | |
| jksoft | 0:72366f00d54c | 27 | #define LEFT2_SENSOR_ON 0x01 |
| jksoft | 0:72366f00d54c | 28 | #define LEFT1_SENSOR_ON 0x02 |
| jksoft | 0:72366f00d54c | 29 | #define CENTER_SENSOR_ON 0x04 |
| jksoft | 0:72366f00d54c | 30 | #define RIGHT1_SENSOR_ON 0x08 |
| jksoft | 0:72366f00d54c | 31 | #define RIGHT2_SENSOR_ON 0x10 |
| jksoft | 0:72366f00d54c | 32 | |
| jksoft | 0:72366f00d54c | 33 | Servo myservo(p21); |
| jksoft | 0:72366f00d54c | 34 | |
| jksoft | 0:72366f00d54c | 35 | BusOut motor1(p5, p6); |
| jksoft | 0:72366f00d54c | 36 | BusOut motor2(p7, p8); |
| jksoft | 0:72366f00d54c | 37 | |
| jksoft | 0:72366f00d54c | 38 | AnalogIn sensor1(p16); |
| jksoft | 0:72366f00d54c | 39 | AnalogIn sensor2(p17); |
| jksoft | 0:72366f00d54c | 40 | AnalogIn sensor3(p18); |
| jksoft | 0:72366f00d54c | 41 | AnalogIn sensor4(p19); |
| jksoft | 0:72366f00d54c | 42 | AnalogIn sensor5(p20); |
| jksoft | 0:72366f00d54c | 43 | |
| jksoft | 0:72366f00d54c | 44 | |
| jksoft | 0:72366f00d54c | 45 | int LineCheck() |
| jksoft | 0:72366f00d54c | 46 | { |
| jksoft | 0:72366f00d54c | 47 | int ret = 0; |
| jksoft | 0:72366f00d54c | 48 | int in[SENSOR_NUM] = { sensor1.read_u16(), |
| jksoft | 0:72366f00d54c | 49 | sensor2.read_u16(), |
| jksoft | 0:72366f00d54c | 50 | sensor3.read_u16(), |
| jksoft | 0:72366f00d54c | 51 | sensor4.read_u16(), |
| jksoft | 0:72366f00d54c | 52 | sensor5.read_u16() |
| jksoft | 0:72366f00d54c | 53 | }; |
| jksoft | 0:72366f00d54c | 54 | |
| jksoft | 0:72366f00d54c | 55 | for(int i = 0 ; i < SENSOR_NUM ; i++ ) |
| jksoft | 0:72366f00d54c | 56 | { |
| jksoft | 0:72366f00d54c | 57 | if( in[i] > SENSOR_TH ) |
| jksoft | 0:72366f00d54c | 58 | { |
| jksoft | 0:72366f00d54c | 59 | ret |= 1 << i; |
| jksoft | 0:72366f00d54c | 60 | } |
| jksoft | 0:72366f00d54c | 61 | } |
| jksoft | 0:72366f00d54c | 62 | |
| jksoft | 0:72366f00d54c | 63 | return(ret); |
| jksoft | 0:72366f00d54c | 64 | } |
| jksoft | 0:72366f00d54c | 65 | |
| jksoft | 0:72366f00d54c | 66 | int main() { |
| jksoft | 0:72366f00d54c | 67 | |
| jksoft | 0:72366f00d54c | 68 | // Motor Stop |
| jksoft | 0:72366f00d54c | 69 | motor1 = 0; |
| jksoft | 0:72366f00d54c | 70 | motor2 = 0; |
| jksoft | 0:72366f00d54c | 71 | |
| jksoft | 0:72366f00d54c | 72 | //Servo Center |
| jksoft | 0:72366f00d54c | 73 | myservo = SERVO_CENTER; |
| jksoft | 0:72366f00d54c | 74 | |
| jksoft | 0:72366f00d54c | 75 | while(1) { |
| jksoft | 0:72366f00d54c | 76 | int value = LineCheck(); |
| jksoft | 0:72366f00d54c | 77 | |
| jksoft | 0:72366f00d54c | 78 | switch(value) |
| jksoft | 0:72366f00d54c | 79 | { |
| jksoft | 0:72366f00d54c | 80 | case LEFT2_SENSOR_ON: // Left Outside |
| jksoft | 0:72366f00d54c | 81 | myservo = 0.2; |
| jksoft | 0:72366f00d54c | 82 | break; |
| jksoft | 0:72366f00d54c | 83 | case LEFT1_SENSOR_ON: // Left Inside |
| jksoft | 0:72366f00d54c | 84 | myservo = 0.4; |
| jksoft | 0:72366f00d54c | 85 | break; |
| jksoft | 0:72366f00d54c | 86 | case CENTER_SENSOR_ON: // Center |
| jksoft | 0:72366f00d54c | 87 | myservo = 0.5; |
| jksoft | 0:72366f00d54c | 88 | break; |
| jksoft | 0:72366f00d54c | 89 | case RIGHT1_SENSOR_ON: // Right Inside |
| jksoft | 0:72366f00d54c | 90 | myservo = 0.6; |
| jksoft | 0:72366f00d54c | 91 | break; |
| jksoft | 0:72366f00d54c | 92 | case RIGHT2_SENSOR_ON: // Right Outside |
| jksoft | 0:72366f00d54c | 93 | myservo = 0.8; |
| jksoft | 0:72366f00d54c | 94 | break; |
| jksoft | 0:72366f00d54c | 95 | default: |
| jksoft | 0:72366f00d54c | 96 | myservo = 0.5; |
| jksoft | 0:72366f00d54c | 97 | break; |
| jksoft | 0:72366f00d54c | 98 | } |
| jksoft | 0:72366f00d54c | 99 | |
| jksoft | 0:72366f00d54c | 100 | // Forward |
| jksoft | 0:72366f00d54c | 101 | motor1 = 0x01; |
| jksoft | 0:72366f00d54c | 102 | motor2 = 0x01; |
| jksoft | 0:72366f00d54c | 103 | } |
| jksoft | 0:72366f00d54c | 104 | } |