main.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 
00004 m3pi m3pi;
00005 
00006 int main() {
00007 
00008     // Parameters that affect the performance
00009     float speed = 0.2;
00010     float correction = 0.1;   
00011     float threshold = 0.5;
00012 
00013     m3pi.locate(0,1);
00014     m3pi.printf("Line Flw");
00015 
00016     wait(2.0);
00017     
00018     m3pi.sensor_auto_calibrate();
00019     
00020     while (1) {
00021 
00022         // -1.0 is far left, 1.0 is far right, 0.0 in the middle
00023         float position_of_line = m3pi.line_position();
00024 
00025         // Line is more than the threshold to the right, slow the left motor
00026         if (position_of_line > threshold) {
00027             m3pi.right_motor(speed);
00028             m3pi.left_motor(speed-correction);
00029         }
00030 
00031         // Line is more than 50% to the left, slow the right motor
00032         else if (position_of_line < -threshold) {
00033             m3pi.left_motor(speed);
00034             m3pi.right_motor(speed-correction);
00035         }
00036 
00037         // Line is in the middle
00038         else {
00039             m3pi.forward(speed);
00040         }
00041     }
00042 }