main.cpp

00001 /***********************************
00002 * name:   func_Bertl    v 0.3
00003 * author: PE HTL BULME
00004 * email:  pe@bulme.at
00005 * description:
00006 *    functions for Bertl The Robot
00007 *
00008 *************************************/
00009 #include "mbed.h"
00010 #include "config.h"
00011 
00012 //bool Start(int speed);
00013 bool Start(float speed);
00014 void ShutOff();
00015 void Move();
00016 bool FrontIsClear();
00017 bool FrontIsClearU(); // with ultra sonic sensor
00018 bool MoveMeasure(int time, int& left, int& right);
00019 bool MoveTicks(int ticks);
00020 void TurnBack();
00021 void TurnLeftTicks(int ticks);
00022 void TurnLeftTime(int ms);
00023 void TurnLeft();
00024 void TurnLedOn(int16_t led);
00025 void TurnLedOff(int16_t led);
00026 void MoveMore(int anzahl);
00027 void Back();
00028 bool WaitUntilButtonPressed();
00029 int BottomIsBlack();    // with line sensor
00030 
00031 // Eigene Funktionsdefinitionen hier
00032 
00033 void LedAround()
00034 {
00035     int i=10;
00036     int led = 0;
00037 
00038     led = LED_FL1;  // 0x01
00039 
00040     while(i-- > 0) {
00041         TurnLedOn(led);
00042         wait(1);
00043         led *= 2;
00044         if(led > 0x80)
00045             led = LED_FL1;  // 0x01
00046     }
00047 }
00048 
00049 // *************  Hauptprogramm ************
00050 int main()
00051 {
00052     Start(0.3);     // speed between 0.2 (low) and 1.0 (normal)
00053     //Move();
00054     while(!BottomIsBlack())
00055     {
00056         MoveTicks(40);
00057     }
00058     TurnLeft();
00059     while(FrontIsClearU())
00060     {
00061         MoveTicks(40);
00062     }
00063 
00064     TurnLeftTicks(200);    // Anzahl der Motorsensorwerte
00065     Move();
00066     ShutOff();
00067     return 0;
00068 }
00069 
00070 // Line sensor at bottom of Bertl
00071 int BottomIsBlack()
00072 {
00073     int detect;
00074     
00075     detect = linesensor;
00076     wait_ms(5);
00077     return detect;
00078 }
00079 
00080 bool Start(float speed)
00081 {
00082     pc.baud(9600);
00083     pc.printf("Hello");
00084 
00085     DEBUG_PRINT("Debug level: %d", (int) DEBUG);
00086     i2c.frequency(40000);       // I2C Frequenz 40kHz
00087     mg1 = mg2 = speed;
00088 //    while(WaitUntilButtonPressed()) {    }
00089     return true;
00090 }
00091 
00092 void ShutOff()
00093 {
00094     MotorR_FORWARD = MotorL_FORWARD = 0;    // motor OFF
00095     MotorR_EN=MotorL_EN=0;                  // motor disable
00096 }
00097 
00098 void Move()
00099 {
00100     int count=0, left=0, right=0;           // initialise variables
00101     MotorR_EN=MotorL_EN=1;                  // both motor ENABLE
00102 
00103     while(count<1000) {
00104         MotorR_FORWARD = MotorL_FORWARD = 1;// both motor forward ON
00105         LED_D10 = SensorL;                  //  LED D10 blinks
00106         if(SensorL == 1)
00107             left++;
00108         if(SensorR == 1)
00109             right++;
00110         LED_D13 = SensorR;                  // LED D13 blinks
00111         count++;
00112         wait_ms(1);                         // wait for 1 ms
00113     }
00114     DEBUG_PRINT("SensorL: %d SensorR: %d\right\n", left, right);
00115 
00116     MotorR_FORWARD = MotorL_FORWARD = 0;    // both motor off
00117     MotorR_EN=MotorL_EN=0;
00118     wait_ms(250);
00119 }
00120 
00121 bool FrontIsClear()
00122 {
00123     char cmd[3];    // array for I2C
00124     int16_t btns;
00125     bool wert;
00126 
00127     cmd[0] = 0x06;
00128     cmd[1] = 0x00;
00129     i2c.write(addr, cmd, 2); // define Port0 = Out
00130 
00131     cmd[0]=0x01;
00132     i2c.write(addr, cmd, 1);
00133     i2c.read(addr|1, cmd, 1);
00134     btns = cmd[0];
00135     if( btns & (BTN_FL|BTN_FM|BTN_FR))
00136         wert = false;
00137     else
00138         wert = true;
00139     DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
00140     return wert;
00141 }
00142 // with ultra sonic sensor 
00143 bool FrontIsClearU()
00144 {
00145     int dist = 0;
00146     usensor.start();
00147     wait_ms(10);
00148     dist=usensor.get_dist_cm();
00149     if(dist < 5)
00150         return false;
00151     else
00152         return true;
00153     DEBUG_PRINT("Distance: %d", dist);
00154 }
00155 /*
00156 use in main():
00157     int left=0, right=0;
00158 
00159     MoveMeasure(1000, left, right);
00160     pc.printf("\r\nleft= %d  right= %d\r\n", left, right);
00161 */
00162 bool MoveMeasure(int time, int& left, int& right)
00163 {
00164     int count=0; //, left=0, right=0;
00165 
00166     MotorR_EN=MotorL_EN=1;
00167     MotorR_FORWARD = MotorL_FORWARD = 1;
00168 
00169     while(count<time) {
00170         MotorR_FORWARD = MotorL_FORWARD = 1;
00171         LED_D10 = SensorL;
00172         if(SensorL == 1)
00173             left++;
00174         if(SensorR == 1)
00175             right++;
00176         LED_D13 = SensorR;
00177         count++;
00178         wait_ms(1);
00179     }
00180     DEBUG_PRINT("SensorL: %d SensorR: %d\right\n", left, right);
00181 
00182     MotorR_FORWARD = MotorL_FORWARD = 0;
00183     MotorR_EN=MotorL_EN=0;
00184     wait_ms(500);
00185     return true;
00186 }
00187 
00188 bool MoveTicks(int ticks)
00189 {
00190     int count=0, left=0, right=0;                   //Variable count auf 0 setzen
00191 
00192     MotorR_EN=MotorL_EN=1;                 // Beide Motoren ENABLE
00193     while(ticks--) {             // mache solang ticks
00194         MotorR_FORWARD = MotorL_FORWARD = 1;  // Beide Motoren vorwärts EIN
00195         LED_D10 = SensorL;   //  LED D10 blinkt
00196         if(SensorL == 1)
00197             left++;
00198         if(SensorR == 1)
00199             right++;
00200         LED_D13 = SensorR;   //  LED D13 blinkt
00201         count++;
00202         wait_ms(1);          //  warte 1 mSekunde
00203     }
00204     DEBUG_PRINT("SensorL: %d SensorR: %d anzahl: %d\right\n", left, right, count);
00205 
00206     MotorR_FORWARD = MotorL_FORWARD = 0;    // Motoren AUS
00207     MotorR_EN=MotorL_EN=0;
00208     //wait_ms(250);
00209     return true;
00210 }
00211 
00212 void TurnBack()
00213 {
00214     MotorR_EN=1;
00215 
00216     MotorR_REVERSE = 1;
00217     wait_ms(750);
00218     MotorR_REVERSE = 0;
00219     MotorR_EN=0;
00220 
00221     wait_ms(250);
00222 }
00223 
00224 void TurnLeftTicks(int ticks)
00225 {
00226     MotorR_EN=1;
00227     MotorL_EN=1;
00228     int left=0, right=0;
00229     while(ticks--) {
00230         MotorR_FORWARD = MotorL_REVERSE = 1;
00231         LED_D10 = SensorL;
00232         if(SensorL == 1)
00233             left++;
00234         if(SensorR == 1)
00235             right++;
00236         LED_D13 = SensorR;
00237         wait_ms(1);
00238     }
00239     DEBUG_PRINT("\right\nTurnLeft: SensorL: %d SensorR: %d \right\n", left, right);
00240 
00241     MotorL_REVERSE = MotorR_FORWARD = 0;
00242     MotorL_EN=MotorR_EN=0;
00243 
00244     wait_ms(250);
00245 }
00246 
00247 void TurnLeftTime(int ms)
00248 {
00249     MotorR_EN=1;            // motor right ENABLE
00250 
00251     MotorR_FORWARD = 1;     // motor right forward ON
00252     wait_ms(ms);            // wait for ms
00253     MotorR_FORWARD = 0;     // motor right OFF
00254     MotorR_EN=0;
00255 
00256     wait_ms(250);           // only to step the robot
00257 }
00258 
00259 void TurnLeft()
00260 {
00261     MotorR_EN=1;            // motor right ENABLE
00262 
00263     MotorR_FORWARD = MotorL_REVERSE = 1;
00264     //MotorR_FORWARD = 1;     // motor right forward ON
00265     wait_ms(500);           // wait 500 ms (90°)
00266     //MotorR_FORWARD = 0;     // motor right forward OFF
00267     //MotorR_EN=0;
00268     MotorL_REVERSE = MotorR_FORWARD = 0;
00269     MotorL_EN=MotorR_EN=0;
00270     wait_ms(250);           // only to step the robot
00271 }
00272 
00273 bool WaitUntilButtonPressed()
00274 {
00275     char cmd[3];
00276     int16_t btns;
00277     bool wert;
00278 
00279     cmd[0] = 0x06;
00280     cmd[1] = 0x00;
00281     i2c.write(addr, cmd, 2); // Define Port0 = Out
00282 
00283     cmd[0]=0x01;
00284     i2c.write(addr, cmd, 1);
00285     i2c.read(addr|1, cmd, 1);
00286     btns = cmd[0];
00287     if( btns & (0xFF))
00288         wert = false;
00289     else
00290         wert = true;
00291     DEBUG_PRINT("\right\nWERT: %d \right\n", wert);
00292     return wert;
00293 }
00294 
00295 void TurnLedOn(int16_t led)
00296 {
00297     char cmd[3];
00298 
00299     cmd[0] = 0x02;
00300     cmd[1] = ~led;
00301     i2c.write(addr, cmd, 2);
00302     wait(0.5);
00303 }
00304 
00305 void TurnLedOff(int16_t led)
00306 {
00307     char cmd[3];
00308 
00309     cmd[0] = 0x02;
00310     cmd[1] = led;
00311     i2c.write(addr, cmd, 2);
00312     wait(0.5);
00313 }
00314 
00315 void ManuelDistancMeasure()
00316 {
00317     int dist = 0;
00318     unsigned char count=0;
00319 
00320     while(count < 20) {
00321         usensor.start();
00322         wait_ms(500);
00323         dist=usensor.get_dist_cm();
00324         pc.printf("\r\nCount =%d",count);
00325         pc.printf("Distance: %d",dist );
00326         count++;
00327 
00328     }
00329 }
00330 
00331 /*
00332 void TurnAllLedOff()
00333 {
00334     TurnLedOff(LED_ALL);
00335 }
00336 
00337 void TurnAllLedOn()
00338 {
00339     TurnLedOn(LED_ALL);
00340 }
00341 
00342 void MoveMore(int anzahl)
00343 {
00344     for(int i=0; i < anzahl; i++)
00345         Move();
00346 }
00347 
00348 void Back()
00349 {
00350     TurnLeftTicks(150);
00351     TurnLeftTicks(150);
00352 }
00353 
00354 void MoveWhileFrontIsClear()
00355 {
00356     while(FrontIsClear()) {
00357         MoveTicks(30);
00358     }
00359 }
00360 
00361 void LedTest()
00362 {
00363     TurnLedOn(LED_FL1 |LED_FR1);
00364     wait(1);
00365     TurnAllLedOn();
00366     wait(1);
00367     TurnLedOff(LED_FL1 |LED_FR1);
00368 }
00369 */