main.cpp

00001 /*
00002  * This program is free software; you can redistribute it and/or modify
00003  * it under the terms of the GNU General Public License as published by
00004  * the Free Software Foundation; either version 3 of the License, or
00005  * (at your option) any later version.
00006  *
00007  * This program is distributed in the hope that it will be useful,
00008  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00009  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00010  * GNU General Public License for more details.
00011  *
00012  * You should have received a copy of the GNU General Public License
00013  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00014  */
00015 
00016 /**
00017  * @file  main.c
00018  * @author     andre moehl
00019  * @date   01/2011
00020  *
00021  *
00022  * Implements the main functionalty and als programs for the XBeat.
00023  */
00024 
00025 /*--- Includes ------------------------------*/
00026 #include "mbed.h"
00027 #include "SRF05.h"
00028 #include "drive.h"
00029 #include "button.h"
00030 
00031 
00032 /*--- Defines -------------------------------*/
00033 #define DEBUG 1
00034 
00035 #define TTLSERIAL 1 //use the ttl(p13,p14) serial port or the native USB serial port if 0
00036 #define BAUDRATE 19200
00037 
00038 #define OFF 0
00039 #define ON 1
00040 
00041 // Battery measure
00042 #define UBAT_R_FACTOR 3.1363636 //resistor divider
00043 //#define UBAT_LOW 66       //6,6V 
00044 #define UBAT_LOW 72         //7,2V - more adequate value to protect lipos
00045 
00046 
00047 // Servo Zero Position Offset
00048 #define OFFSET_L -35
00049 #define OFFSET_R 45
00050 
00051 
00052 
00053 /*--- Global Variables ----------------------*/
00054 /* Hardware Definitions */
00055 DigitalOut led1(LED1);      ///< Leds on mbed board
00056 DigitalOut led2(LED2);
00057 DigitalOut led3(LED3);
00058 DigitalOut led4(LED4);
00059 DigitalOut Speaker(p12);    ///< Loudspeaker
00060 AnalogIn   ubat(p15);       ///< LiPo Voltage Measurement
00061 DigitalOut T6(p29);         ///< FET for Bottom IR-LEDs
00062 DigitalOut T7(p30);         ///< FET for Side IR-LEDs
00063 AnalogIn   IRL(p16);        ///< IR-Transistor Bottom Left
00064 AnalogIn   IRM(p17);        ///< IR-Transistor Bottom Middle
00065 AnalogIn   IRR(p18);        ///< IR-Transistor Bottom Right
00066 AnalogIn Left(p19);         ///< IR-Transistor Left Side
00067 AnalogIn Right(p20);        ///< IR-Transistor Right Side
00068 Button SW1(p10);            ///< Button Outer Side
00069 Button SW2(p9);             ///< Button Inner Side
00070 
00071 #ifdef TTLSERIAL
00072 Serial pc (p13,p14);        ///< Serial Interface via TTL Port
00073 #else
00074 Serial pc(USBTX, USBRX);  ///< Serial Intervace via USB Port (tx, rx)
00075 #endif
00076 
00077 /** Timer and Tickers */
00078 Ticker LiPoTicker;  //ticker for checking LiPoVoltage
00079 Ticker tick_1ms;    //Ticker for checking Button
00080 
00081 /*--- Flags ---------------------------------*/
00082 
00083 int SW1_state=0;    // Current State of SW1
00084 int SW1_old=0;      // Previous State of SW1
00085 int SW2_state=0;    // Current State of SW2
00086 int SW2_old=0;      // Previous State of SW2
00087 int start=0;        // indicate to start a program
00088 int stop=1;         // indicate to stop a program
00089 int programm_counter=1; // Counts the number of SW2 samples to select a program
00090 uint8_t firstUnderVoltage;  // set to 1 when the battery measure is first time under the limit
00091 
00092 
00093 /**--- predeclarared Functions --------------*/
00094 void setLed(int led);
00095 float getLiPoVoltage();
00096 
00097 /**--- Functions ----------------------------*/
00098 
00099 ////// PROGRAMM LINE FOLLOW ///////////////////////////////////////////
00100 
00101 #define LFDUTYCYCLE 0.001
00102 /**
00103 * @brief Programm LineFollow follows lines on paper. Uses the IR-Transistors to detect.
00104 * @parameter mode Fast or Slow
00105 */
00106 void LineFollow() {
00107     Drive XBeat(p21,p22);                                       //Left, Right Servo Motor
00108     int i;
00109     int ML=0, MR=0;                                             //Motor set-points
00110     int MaxSpeed=0;                                             ///< Maximum Speed in Percent
00111     unsigned short left=0, right=0, mid=0;                      // current measured values
00112     unsigned short  soll_left=0, soll_right=0, soll_mid=0;      // must values
00113     signed int error_r=0,error_l=0,error_m=0;                   // setpoint errors
00114     signed int prev_error_r=0, prev_error_l=0, prev_error_m=0;  // previous setpoint errors
00115     signed int dmeas_r=0, dmeas_l=0, dmeas_m=0;                 // derive error
00116     signed int pid_r=0, pid_l=0, pid_m=0;                       // calced controller values
00117 
00118 
00119     MaxSpeed = 100;                                             //Set Maximum Line Follow Speed
00120 
00121     pc.printf("Program \"Line Follow\"\n\r\n\r");
00122     XBeat.setoffset(OFFSET_L,OFFSET_R);
00123     T6 = ON; //switch IR-Leds on
00124 
00125     //read values for calc the set point values
00126     soll_left = IRL.read_u16()/100;
00127     soll_right = IRM.read_u16()/100;
00128     soll_mid = IRM.read_u16()/100;
00129     for (i=30;i>0;i--) {
00130         soll_left = (soll_left + IRL.read_u16()/100)/2;
00131         soll_right = (soll_right + IRR.read_u16()/100)/2;
00132         soll_mid = (soll_mid + IRM.read_u16()/100)/2;
00133     }
00134     //debug
00135     pc.printf("Sollwert Links: %d\n\r", soll_left);
00136     pc.printf("Sollwert Rechts: %d\n\r", soll_right);
00137     wait(2);
00138     //peep
00139     Speaker = 1;
00140     wait (0.1);
00141     Speaker = 0;
00142     //acceleration
00143     int ms = 500/MaxSpeed;
00144     for (i=0; i<MaxSpeed; i+=2) {
00145         XBeat.move(i,i);
00146         wait_ms(ms);
00147     }
00148 
00149     //drive until pressing stop button
00150     while (!stop) {
00151         //set SetPoint
00152         MR = MaxSpeed;
00153         ML = MaxSpeed;
00154 
00155         //get sensor data
00156         left = IRL.read_u16()/100;
00157         right = IRR.read_u16()/100;
00158         mid = IRM.read_u16()/100;
00159 
00160         //calc controller (PD)
00161         error_l = soll_left - left;
00162         dmeas_l = (error_l - prev_error_l)/2;
00163         prev_error_l = error_l;
00164         error_r = soll_right - right;
00165         dmeas_r = (error_r - prev_error_r)/2;
00166         prev_error_r = error_r;
00167         error_m = soll_mid - mid;
00168         dmeas_m = (error_m - prev_error_m)/2;
00169         prev_error_m = error_m;
00170 
00171         pid_l = (2*error_l + dmeas_l)/20;
00172         pid_r = (2*error_r + dmeas_r)/20;
00173         pid_m = (2*error_m + dmeas_m)/10;
00174 
00175         //Limit controller output
00176         if (pid_l >= MaxSpeed) pid_l = MaxSpeed;
00177         if (pid_l <= -MaxSpeed) pid_l = -MaxSpeed;
00178         if (pid_r >= MaxSpeed) pid_r = MaxSpeed;
00179         if (pid_r <= -MaxSpeed) pid_r = -MaxSpeed;
00180         //debug
00181         pc.printf("pidL %d, pidR%d\n\r", pid_l, pid_r);
00182 
00183         if (pid_l > 2) ML = ML + pid_m;
00184         if (pid_r > 2) MR = MR + pid_m;
00185 
00186         ML = ML - pid_l;// - pid_r/2;
00187         MR = MR - pid_r;// - pid_l/2;
00188         if (ML > MaxSpeed) {
00189             ML = MaxSpeed;
00190             MR = MR + pid_l;
00191         }
00192         if (MR > MaxSpeed) {
00193             MR = MaxSpeed;
00194             ML = ML + pid_r;
00195         }
00196         //Bottom Limit Motor values
00197         if (ML < -10) ML = -10;
00198         if (MR < -10) MR = -10;
00199 
00200         pc.printf("ML %d, MR%d\n\r", ML, MR);
00201 
00202         //set Motor values
00203         XBeat.move(ML, MR); //set motor speed
00204         wait(LFDUTYCYCLE);
00205     }
00206     //Finished
00207     T6 = OFF;
00208     start = 0;
00209     stop = 1;
00210     pc.printf("Program \"Line Follow\" stopped\n\r");
00211 }
00212 
00213 ////// PROGRAMM REMOTE CONTROL //////////////////////////////////////////
00214 
00215 /**
00216 * @brief Control the XBeat via remote
00217 */
00218 void RemoteControl() {
00219     Drive XBeat(p22,p21);       //Left, Right Servo Motor
00220     pc.printf("Program \"Remote Control\" started\n\r");
00221     XBeat.setoffset(OFFSET_L,OFFSET_R);
00222     pc.printf("Currently not implemented.\n\r");
00223     start = 0;
00224     stop = 1;
00225     pc.printf("Program \"Remote Control\" stopped\n\r");
00226 }
00227 
00228 
00229 ////// PROGRAMM MAZE SOLVER /////////////////////////////////////////////
00230 
00231 #define MAZE_SPEED 60
00232 #define MAZEDUTYCYCLE 0.01
00233 /**
00234  * @brief drives the XBeat thru a maze
00235  * made for 27C3 Maze Logo
00236  */
00237 void MazeSolver() {
00238     Drive XBeat(p22,p21);       //Left, Right Servo Motor
00239     pc.printf("Program \"Maze Solver\" started\n\r");
00240     XBeat.setoffset(OFFSET_L,OFFSET_R);
00241     pc.printf("The Implementation is currently not finished. I working on it and release this later.\n\r");
00242     start = 0;
00243     stop = 1;
00244     pc.printf("Program \"Maze Solver\" stopped\n\r");
00245 }
00246 
00247 ///// PROGRAMM DRIVE AND TURN /////////////////////////////////////////////
00248 
00249 void driver() {
00250     SRF05 srf(p6, p5);          // Front Sensor
00251     Drive XBeat(p21,p22);       // Left, Right Servo Motor
00252     signed int m1=0,m2=0;       // Motor Set values
00253     float front=0, front_old=0; // measured value from front sensor
00254     int turn=0;                 // flag for indicate a turn
00255     int ticks=0;                // loop counter for turn
00256 
00257     pc.printf("\n\rJust Moving\n\r\n\r");
00258     XBeat.setoffset(OFFSET_L,OFFSET_R);
00259 
00260     while (!stop) {
00261         if (!turn) {
00262             if ((front < front_old) && (front > 8.0)) { //close to a front wall, slow stopping
00263                 if (m1 < 0) m1 = m1-1;
00264                 if (m2 < 0) m2 = m2-1;
00265                 pc.printf("1\n\r");
00266             } else {
00267                 if (m1 < 100) m1++;
00268                 if (m2 < 100) m2++;
00269             }
00270             front_old = front;
00271         }
00272 
00273         if (turn) {
00274             ticks++;
00275             if (ticks <= 300) {
00276                 if (m1 < 50) m1++;
00277                 if (m2 > -50) m2--;
00278             } else {
00279                 if (m1 >= 0) m1--;
00280                 if (m2 <= 0) m2++;
00281                 if (m1 == 0 || m2 == 0) {
00282                     turn=0;
00283                     ticks=0;
00284                 }
00285             }
00286         }
00287 
00288         if (front < 8.0 && !turn) {
00289             m1=0;
00290             m2=0;
00291             turn=1;
00292             ticks=0;
00293         }
00294         XBeat.move(m1,m2);
00295     }
00296     start = 0;
00297     stop = 1;
00298     pc.printf("Program \"Driving\" stopped\n\r");
00299 }
00300 
00301 
00302 /////// Sensor Data ///////////////////////////////////////////////////////
00303 
00304 /**
00305 *   @brief Writing Sensor Data to UART, for debugging purpose
00306 */
00307 void SensorData() {
00308     SRF05 srf(p6, p5);
00309     T6 = ON;
00310     T7 = ON;
00311     pc.printf("Sensor Data\n\r\n\r");
00312     while (!stop) {
00313         pc.printf("IR-Left %d, IR-Right %d, SRF05 %f\n\r", Left.read_u16()/100, Right.read_u16()/100, srf.read());
00314         pc.printf("IRL %d, IRM %d, IRR %d\n\r", IRL.read_u16()/100, IRM.read_u16()/100, IRR.read_u16()/100);
00315         pc.printf("LiPo Voltage %f\n\r\n\r", getLiPoVoltage() );
00316         wait (1);
00317     }
00318     T7 = OFF;
00319     T6 = OFF;
00320     start = 0;
00321     stop = 1;
00322 }
00323 
00324 
00325 /////// RECALLED FUNCTIONS ////////////////////////////////////
00326 /**
00327  * @brief function called ervery 1ms to read the button states
00328  */
00329 void checkButtons () {
00330     //int c=0;
00331     if (SW1) {
00332         SW1_state = 1;
00333     } else SW1_state = 0;
00334     if (SW1_old == 1 && SW1_state == 0) {       //falling edge
00335         /** Add Code to do when Button 1 is pressed */
00336         start = !start;
00337         stop = !start;
00338     }
00339     SW1_old = SW1_state;
00340 
00341     if (SW2) SW2_state = 1;
00342     else SW2_state = 0;
00343     if (SW2_old == 1 && SW2_state == 0) {       //falling edge
00344         /** Add Code to do when Button 2 is pressed */
00345         if (stop) {
00346             if (programm_counter < 5) programm_counter++;
00347             else programm_counter = 1;
00348 
00349             switch (programm_counter) {
00350                 case 1:
00351                     pc.printf("Programm %d:  Line Follow\n\r", programm_counter );
00352                     break;
00353                 case 2:
00354                     pc.printf("Programm %d:  Maze Solver\n\r", programm_counter );
00355                     break;
00356                 case 3:
00357                     pc.printf("Programm %d:  Driving\n\r", programm_counter );
00358                     break;
00359                 case 4:
00360                     pc.printf("Programm %d:  Remote Control\n\r", programm_counter );
00361                     break;
00362                 case 5:
00363                     pc.printf("Programm %d:  Sensor Data\n\r", programm_counter );
00364                     break;
00365             }
00366             if (programm_counter == 5) {
00367                 led1=1;
00368                 led4=1;
00369             } else    setLed(programm_counter);
00370         }
00371     }
00372     SW2_old = SW2_state;
00373 }
00374 
00375 
00376 
00377 /** @brief calculates the battery voltage
00378 *   @return real voltage als float */
00379 float getLiPoVoltage() {
00380     return ((ubat*UBAT_R_FACTOR*3.3)+0.79);
00381 }
00382 
00383 /**@brief function called every 1s to check LiPo Voltage */
00384 void LiPo_Warning() {
00385     int c = 0;
00386     //LiPo Undervoltage Emergency Stop
00387     if ((getLiPoVoltage()*10) < UBAT_LOW) {
00388 
00389         start=0;
00390         stop=1;
00391         if (firstUnderVoltage) { //need to return 1st time to stop the motors
00392             firstUnderVoltage = 0;
00393             return;
00394         } else {
00395             while (1) { //stay here
00396                 Speaker = 1;
00397                 led4 = 1;
00398                 wait(0.1);
00399                 led4 = 0;
00400                 Speaker = 0;
00401                 wait(1);
00402                 if (c++ == 5) {
00403                     pc.printf("LiPo Voltage: %fV\n\r",getLiPoVoltage());
00404                     c=0;
00405                 }
00406             }
00407         }
00408     }
00409 }
00410 
00411 //////// HELPER ///////////////////////////////////////////////////////////
00412 /** @ brief set the LEDs on the mbed dev board
00413  * @parameter led - LED Number (1-4)
00414  */
00415 void setLed(int led) {
00416     led1=OFF;
00417     led2=OFF;
00418     led3=OFF;
00419     led4=OFF;
00420 
00421     switch (led) {
00422         case 1:
00423             led1 = ON;
00424             break;
00425         case 2:
00426             led2 = ON;
00427             break;
00428         case 3:
00429             led3 = ON;
00430             break;
00431         case 4:
00432             led4 = ON;
00433             break;
00434     }
00435 }
00436 
00437 
00438 
00439 
00440 /** @brief Initializations */
00441 void init() {
00442     pc.baud(BAUDRATE);                          //set uart baudrate
00443     tick_1ms.attach_us(&checkButtons, 1000);    //ervery 1ms
00444     firstUnderVoltage = 1;                      // set flag
00445     LiPoTicker.attach(&LiPo_Warning,1);         //check LiPo voltage every second
00446 }
00447 
00448 ///////// MAIN  ///////////////////////////////////////////////////////////
00449 
00450 /**
00451 *   @brief main function
00452 */
00453 int main() {
00454     init();
00455     setLed(programm_counter);
00456     pc.printf("---- XBeat ----\n\r\n\r");
00457     pc.printf("Initial Program 1: Line Follow\n\r");
00458     while (1) {
00459         wait(0.1);
00460         if (start) {
00461             switch (programm_counter) {
00462                 case 1:
00463                     LineFollow();
00464                     setLed(programm_counter);
00465                     break;
00466                 case 2:
00467                     MazeSolver();
00468                     setLed(programm_counter);
00469                     break;
00470                 case 3:
00471                     driver();
00472                     setLed(programm_counter);
00473                     break;
00474                 case 4:
00475                     RemoteControl();
00476                     setLed(programm_counter);
00477                     break;
00478                 case 5:
00479                     SensorData();
00480                     setLed(programm_counter++);
00481                     break;
00482                 default:
00483                     break;
00484             }
00485         }
00486     }
00487 }
00488