Junichi Katsu
ライントレーサーのテストプログラムです。 コンパイルのみ 動作未チェック
Revision 0:72366f00d54c, committed 2012-04-01
- Comitter:
- jksoft
- Date:
- Sun Apr 01 06:48:54 2012 +0000
- Commit message:
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Sun Apr 01 06:48:54 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Apr 01 06:48:54 2012 +0000
@@ -0,0 +1,104 @@
+// Line Trace Test Program
+// By JKSOFT
+
+// Line Sensor Port
+// Left Center Right
+// p16 p17 p18 p19 p20
+// 0x01 0x02 0x04 0x08 0x10
+
+// Motor Port
+// Left motor1(M1) p5,p6 Forward = 0x01 Back = 0x02
+// Right motor2(M2) p7,p8 Forward = 0x01 Back = 0x02
+
+// Servo Port
+// p21
+// Center 0.5
+// Left Max 0.0
+// Right Max 1.0
+
+#include "mbed.h"
+#include "Servo.h"
+
+#define SENSOR_TH 0x8000
+#define SENSOR_NUM 5
+
+#define SERVO_CENTER 0.5
+
+#define LEFT2_SENSOR_ON 0x01
+#define LEFT1_SENSOR_ON 0x02
+#define CENTER_SENSOR_ON 0x04
+#define RIGHT1_SENSOR_ON 0x08
+#define RIGHT2_SENSOR_ON 0x10
+
+Servo myservo(p21);
+
+BusOut motor1(p5, p6);
+BusOut motor2(p7, p8);
+
+AnalogIn sensor1(p16);
+AnalogIn sensor2(p17);
+AnalogIn sensor3(p18);
+AnalogIn sensor4(p19);
+AnalogIn sensor5(p20);
+
+
+int LineCheck()
+{
+ int ret = 0;
+ int in[SENSOR_NUM] = { sensor1.read_u16(),
+ sensor2.read_u16(),
+ sensor3.read_u16(),
+ sensor4.read_u16(),
+ sensor5.read_u16()
+ };
+
+ for(int i = 0 ; i < SENSOR_NUM ; i++ )
+ {
+ if( in[i] > SENSOR_TH )
+ {
+ ret |= 1 << i;
+ }
+ }
+
+ return(ret);
+}
+
+int main() {
+
+ // Motor Stop
+ motor1 = 0;
+ motor2 = 0;
+
+ //Servo Center
+ myservo = SERVO_CENTER;
+
+ while(1) {
+ int value = LineCheck();
+
+ switch(value)
+ {
+ case LEFT2_SENSOR_ON: // Left Outside
+ myservo = 0.2;
+ break;
+ case LEFT1_SENSOR_ON: // Left Inside
+ myservo = 0.4;
+ break;
+ case CENTER_SENSOR_ON: // Center
+ myservo = 0.5;
+ break;
+ case RIGHT1_SENSOR_ON: // Right Inside
+ myservo = 0.6;
+ break;
+ case RIGHT2_SENSOR_ON: // Right Outside
+ myservo = 0.8;
+ break;
+ default:
+ myservo = 0.5;
+ break;
+ }
+
+ // Forward
+ motor1 = 0x01;
+ motor2 = 0x01;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Apr 01 06:48:54 2012 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/737756e0b479