main.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 
00004 #define BASE_SPEED 0.25
00005 #define K_P 0.25
00006 
00007 // The pi object lets us access the robot sensors, motors and LCD.
00008 // See the user guide at: http://mbed.org/cookbook/m3pi and
00009 //  http://mbed.org/users/chris/code/m3pi/docs/4b7d6ea9b35b/classm3pi.html
00010 m3pi pi;
00011 
00012 int main() {
00013     float left_speed, right_speed, line_position;
00014     
00015     // Display "GGC 2012" so users know they have the correct program
00016     pi.locate(0,0);
00017     pi.printf("GGC 2012");
00018     
00019     wait(0.5);
00020 
00021     // Auto-align the sensors on the bottom of the robot
00022     pi.sensor_auto_calibrate();
00023 
00024     while(1)
00025     {   // This implements proportional feedback for driving the motors
00026     
00027         // Get the position of the line: -1.0 is off to the left;
00028         //  +1.0 is off to the right.
00029         line_position = pi.line_position();
00030         
00031         // Speed for each motor can be -1.0 to +1.0. With the default values
00032         //  it is limited to between 0 and 0.5.
00033         left_speed  = BASE_SPEED - line_position*K_P;
00034         right_speed = BASE_SPEED + line_position*K_P;
00035         
00036         // Send new speeds to the motor controller
00037         pi.left_motor(left_speed);
00038         pi.right_motor(right_speed);
00039     }
00040 
00041     pi.stop();
00042 }