utils.h

00001 
00002 PwmOut left_speed(D6);
00003 DigitalOut INA1(D7);
00004 DigitalOut INA2(D8);
00005 
00006 PwmOut right_speed(D9);
00007 DigitalOut INB1(D10);
00008 DigitalOut INB2(D11);
00009 
00010 AnalogIn DIST(A6);
00011 void motorL(int d)
00012 {
00013     if (d > 0)
00014     {
00015         INA1 = 1;
00016         INA2 = 0;
00017     }
00018     else if (d < 0)
00019     {
00020         INA1 = 0;
00021         INA2 = 1;
00022     }
00023     else
00024     {
00025         INA1 = 1;
00026         INA2 = 1;
00027     }
00028 }
00029 void motorR(int d)
00030 {
00031     if (d > 0)
00032     {
00033         INB1 = 1;
00034         INB2 = 0;
00035     }
00036     else if (d < 0)
00037     {
00038         INB1 = 0;
00039         INB2 = 1;
00040     }
00041     else
00042     {
00043         INB1 = 1;
00044         INB2 = 1;
00045     }
00046 }
00047 void stop()
00048 {
00049     motorL(0);
00050     motorR(0);
00051 }
00052 void go_straight(float t = -1.0)
00053 {
00054     motorL(1);
00055     motorR(1);
00056     if (t > 0.0)
00057     {
00058         wait(t);
00059         stop();
00060     }
00061 }
00062 void go_back(float t = -1.0)
00063 {
00064     motorL(-1);
00065     motorR(-1);
00066     if (t > 0.0)
00067     {
00068         wait(t);
00069         stop();
00070     }
00071 }
00072 void turn_left(float t = -1.0)
00073 {
00074     motorL(1);
00075     motorR(-1);
00076     if (t > 0.0)
00077     {
00078         wait(t);
00079         stop();
00080     }
00081 }
00082 void turn_right(float t = -1.0)
00083 {
00084     motorL(-1);
00085     motorR(1);
00086     if (t > 0.0)
00087     {
00088         wait(t);
00089         stop();
00090     }
00091 }
00092 int get_distance()
00093 {
00094     float vol = DIST.read();
00095     return (int)(76.0 / vol);
00096 }