yasuo hayashibara / Mbed 2 deprecated robocon_line_tracer

This is a line tracer program for the Robocon Magazine.

Dependencies:   mbed

main.cpp@0:060fa34725a8, 2013-03-17 (annotated)

Committer:
yasuohayashibara
Date:
Sun Mar 17 10:01:35 2013 +0000
Revision:
0:060fa34725a8
This is a program for the Robocon Magazine.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yasuohayashibara 0:060fa34725a8 1 #include "mbed.h"
yasuohayashibara 0:060fa34725a8 2 #include <algorithm>
yasuohayashibara 0:060fa34725a8 3
yasuohayashibara 0:060fa34725a8 4 PwmOut motor1(p26);
yasuohayashibara 0:060fa34725a8 5 PwmOut motor2(p25);
yasuohayashibara 0:060fa34725a8 6 PwmOut motor3(p24);
yasuohayashibara 0:060fa34725a8 7 PwmOut motor4(p23);
yasuohayashibara 0:060fa34725a8 8 DigitalOut ledRight(LED1);
yasuohayashibara 0:060fa34725a8 9 DigitalOut ledLeft (LED2);
yasuohayashibara 0:060fa34725a8 10 AnalogIn photoRight(p15);
yasuohayashibara 0:060fa34725a8 11 AnalogIn photoLeft (p16);
yasuohayashibara 0:060fa34725a8 12 DigitalIn sw(p9);
yasuohayashibara 0:060fa34725a8 13
yasuohayashibara 0:060fa34725a8 14 #define PERIOD 0.00005
yasuohayashibara 0:060fa34725a8 15
yasuohayashibara 0:060fa34725a8 16 void init()
yasuohayashibara 0:060fa34725a8 17 {
yasuohayashibara 0:060fa34725a8 18 motor1.period(PERIOD);
yasuohayashibara 0:060fa34725a8 19 motor2.period(PERIOD);
yasuohayashibara 0:060fa34725a8 20 motor3.period(PERIOD);
yasuohayashibara 0:060fa34725a8 21 motor4.period(PERIOD);
yasuohayashibara 0:060fa34725a8 22 sw.mode(PullUp);
yasuohayashibara 0:060fa34725a8 23 }
yasuohayashibara 0:060fa34725a8 24
yasuohayashibara 0:060fa34725a8 25 void motor(int right, int left)
yasuohayashibara 0:060fa34725a8 26 {
yasuohayashibara 0:060fa34725a8 27 right = min(max(right, -100), 100);
yasuohayashibara 0:060fa34725a8 28 left = min(max(left , -100), 100);
yasuohayashibara 0:060fa34725a8 29 motor1.pulsewidth(max(-PERIOD * right / 100, 0.0));
yasuohayashibara 0:060fa34725a8 30 motor2.pulsewidth(max( PERIOD * right / 100, 0.0));
yasuohayashibara 0:060fa34725a8 31 motor3.pulsewidth(max( PERIOD * left / 100, 0.0));
yasuohayashibara 0:060fa34725a8 32 motor4.pulsewidth(max(-PERIOD * left / 100, 0.0));
yasuohayashibara 0:060fa34725a8 33 }
yasuohayashibara 0:060fa34725a8 34
yasuohayashibara 0:060fa34725a8 35 int main() {
yasuohayashibara 0:060fa34725a8 36 int run = 0, right, left, forward = 70;
yasuohayashibara 0:060fa34725a8 37 float error, gain = 70;
yasuohayashibara 0:060fa34725a8 38 init(); motor(0, 0);
yasuohayashibara 0:060fa34725a8 39 while(1){
yasuohayashibara 0:060fa34725a8 40 error = photoRight.read() - photoLeft.read();
yasuohayashibara 0:060fa34725a8 41 ledRight = (error > 0);
yasuohayashibara 0:060fa34725a8 42 ledLeft = (error < 0);
yasuohayashibara 0:060fa34725a8 43 right = forward + gain * error;
yasuohayashibara 0:060fa34725a8 44 left = forward - gain * error;
yasuohayashibara 0:060fa34725a8 45 if (!sw) { run ^= 1; wait(0.5); }
yasuohayashibara 0:060fa34725a8 46 if (run) motor(right, left);
yasuohayashibara 0:060fa34725a8 47 else motor(0, 0);
yasuohayashibara 0:060fa34725a8 48 wait(0.01);
yasuohayashibara 0:060fa34725a8 49 }
yasuohayashibara 0:060fa34725a8 50 }