yasuo hayashibara
This is a line tracer program for the Robocon Magazine.
Revision 0:060fa34725a8, committed 2013-03-17
- Comitter:
- yasuohayashibara
- Date:
- Sun Mar 17 10:01:35 2013 +0000
- Commit message:
- This is a program for the Robocon Magazine.
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Mar 17 10:01:35 2013 +0000
@@ -0,0 +1,50 @@
+#include "mbed.h"
+#include <algorithm>
+
+PwmOut motor1(p26);
+PwmOut motor2(p25);
+PwmOut motor3(p24);
+PwmOut motor4(p23);
+DigitalOut ledRight(LED1);
+DigitalOut ledLeft (LED2);
+AnalogIn photoRight(p15);
+AnalogIn photoLeft (p16);
+DigitalIn sw(p9);
+
+#define PERIOD 0.00005
+
+void init()
+{
+ motor1.period(PERIOD);
+ motor2.period(PERIOD);
+ motor3.period(PERIOD);
+ motor4.period(PERIOD);
+ sw.mode(PullUp);
+}
+
+void motor(int right, int left)
+{
+ right = min(max(right, -100), 100);
+ left = min(max(left , -100), 100);
+ motor1.pulsewidth(max(-PERIOD * right / 100, 0.0));
+ motor2.pulsewidth(max( PERIOD * right / 100, 0.0));
+ motor3.pulsewidth(max( PERIOD * left / 100, 0.0));
+ motor4.pulsewidth(max(-PERIOD * left / 100, 0.0));
+}
+
+int main() {
+ int run = 0, right, left, forward = 70;
+ float error, gain = 70;
+ init(); motor(0, 0);
+ while(1){
+ error = photoRight.read() - photoLeft.read();
+ ledRight = (error > 0);
+ ledLeft = (error < 0);
+ right = forward + gain * error;
+ left = forward - gain * error;
+ if (!sw) { run ^= 1; wait(0.5); }
+ if (run) motor(right, left);
+ else motor(0, 0);
+ wait(0.01);
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Mar 17 10:01:35 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b \ No newline at end of file