File content as of revision 0:badcd0d61c7b:

/*
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file  main.c
 * @author     andre moehl
 * @date   01/2011
 *
 *
 * Implements the main functionalty and als programs for the XBeat.
 */

/*--- Includes ------------------------------*/
#include "mbed.h"
#include "SRF05.h"
#include "drive.h"
#include "button.h"


/*--- Defines -------------------------------*/
#define DEBUG 1

#define TTLSERIAL 1 //use the ttl(p13,p14) serial port or the native USB serial port if 0
#define BAUDRATE 19200

#define OFF 0
#define ON 1

// Battery measure
#define UBAT_R_FACTOR 3.1363636 //resistor divider
//#define UBAT_LOW 66       //6,6V 
#define UBAT_LOW 72         //7,2V - more adequate value to protect lipos


// Servo Zero Position Offset
#define OFFSET_L -35
#define OFFSET_R 45



/*--- Global Variables ----------------------*/
/* Hardware Definitions */
DigitalOut led1(LED1);      ///< Leds on mbed board
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut Speaker(p12);    ///< Loudspeaker
AnalogIn   ubat(p15);       ///< LiPo Voltage Measurement
DigitalOut T6(p29);         ///< FET for Bottom IR-LEDs
DigitalOut T7(p30);         ///< FET for Side IR-LEDs
AnalogIn   IRL(p16);        ///< IR-Transistor Bottom Left
AnalogIn   IRM(p17);        ///< IR-Transistor Bottom Middle
AnalogIn   IRR(p18);        ///< IR-Transistor Bottom Right
AnalogIn Left(p19);         ///< IR-Transistor Left Side
AnalogIn Right(p20);        ///< IR-Transistor Right Side
Button SW1(p10);            ///< Button Outer Side
Button SW2(p9);             ///< Button Inner Side

#ifdef TTLSERIAL
Serial pc (p13,p14);        ///< Serial Interface via TTL Port
#else
Serial pc(USBTX, USBRX);  ///< Serial Intervace via USB Port (tx, rx)
#endif

/** Timer and Tickers */
Ticker LiPoTicker;  //ticker for checking LiPoVoltage
Ticker tick_1ms;    //Ticker for checking Button

/*--- Flags ---------------------------------*/

int SW1_state=0;    // Current State of SW1
int SW1_old=0;      // Previous State of SW1
int SW2_state=0;    // Current State of SW2
int SW2_old=0;      // Previous State of SW2
int start=0;        // indicate to start a program
int stop=1;         // indicate to stop a program
int programm_counter=1; // Counts the number of SW2 samples to select a program
uint8_t firstUnderVoltage;  // set to 1 when the battery measure is first time under the limit


/**--- predeclarared Functions --------------*/
void setLed(int led);
float getLiPoVoltage();

/**--- Functions ----------------------------*/

////// PROGRAMM LINE FOLLOW ///////////////////////////////////////////

#define LFDUTYCYCLE 0.001
/**
* @brief Programm LineFollow follows lines on paper. Uses the IR-Transistors to detect.
* @parameter mode Fast or Slow
*/
void LineFollow() {
    Drive XBeat(p21,p22);                                       //Left, Right Servo Motor
    int i;
    int ML=0, MR=0;                                             //Motor set-points
    int MaxSpeed=0;                                             ///< Maximum Speed in Percent
    unsigned short left=0, right=0, mid=0;                      // current measured values
    unsigned short  soll_left=0, soll_right=0, soll_mid=0;      // must values
    signed int error_r=0,error_l=0,error_m=0;                   // setpoint errors
    signed int prev_error_r=0, prev_error_l=0, prev_error_m=0;  // previous setpoint errors
    signed int dmeas_r=0, dmeas_l=0, dmeas_m=0;                 // derive error
    signed int pid_r=0, pid_l=0, pid_m=0;                       // calced controller values


    MaxSpeed = 100;                                             //Set Maximum Line Follow Speed

    pc.printf("Program \"Line Follow\"\n\r\n\r");
    XBeat.setoffset(OFFSET_L,OFFSET_R);
    T6 = ON; //switch IR-Leds on

    //read values for calc the set point values
    soll_left = IRL.read_u16()/100;
    soll_right = IRM.read_u16()/100;
    soll_mid = IRM.read_u16()/100;
    for (i=30;i>0;i--) {
        soll_left = (soll_left + IRL.read_u16()/100)/2;
        soll_right = (soll_right + IRR.read_u16()/100)/2;
        soll_mid = (soll_mid + IRM.read_u16()/100)/2;
    }
    //debug
    pc.printf("Sollwert Links: %d\n\r", soll_left);
    pc.printf("Sollwert Rechts: %d\n\r", soll_right);
    wait(2);
    //peep
    Speaker = 1;
    wait (0.1);
    Speaker = 0;
    //acceleration
    int ms = 500/MaxSpeed;
    for (i=0; i<MaxSpeed; i+=2) {
        XBeat.move(i,i);
        wait_ms(ms);
    }

    //drive until pressing stop button
    while (!stop) {
        //set SetPoint
        MR = MaxSpeed;
        ML = MaxSpeed;

        //get sensor data
        left = IRL.read_u16()/100;
        right = IRR.read_u16()/100;
        mid = IRM.read_u16()/100;

        //calc controller (PD)
        error_l = soll_left - left;
        dmeas_l = (error_l - prev_error_l)/2;
        prev_error_l = error_l;
        error_r = soll_right - right;
        dmeas_r = (error_r - prev_error_r)/2;
        prev_error_r = error_r;
        error_m = soll_mid - mid;
        dmeas_m = (error_m - prev_error_m)/2;
        prev_error_m = error_m;

        pid_l = (2*error_l + dmeas_l)/20;
        pid_r = (2*error_r + dmeas_r)/20;
        pid_m = (2*error_m + dmeas_m)/10;

        //Limit controller output
        if (pid_l >= MaxSpeed) pid_l = MaxSpeed;
        if (pid_l <= -MaxSpeed) pid_l = -MaxSpeed;
        if (pid_r >= MaxSpeed) pid_r = MaxSpeed;
        if (pid_r <= -MaxSpeed) pid_r = -MaxSpeed;
        //debug
        pc.printf("pidL %d, pidR%d\n\r", pid_l, pid_r);

        if (pid_l > 2) ML = ML + pid_m;
        if (pid_r > 2) MR = MR + pid_m;

        ML = ML - pid_l;// - pid_r/2;
        MR = MR - pid_r;// - pid_l/2;
        if (ML > MaxSpeed) {
            ML = MaxSpeed;
            MR = MR + pid_l;
        }
        if (MR > MaxSpeed) {
            MR = MaxSpeed;
            ML = ML + pid_r;
        }
        //Bottom Limit Motor values
        if (ML < -10) ML = -10;
        if (MR < -10) MR = -10;

        pc.printf("ML %d, MR%d\n\r", ML, MR);

        //set Motor values
        XBeat.move(ML, MR); //set motor speed
        wait(LFDUTYCYCLE);
    }
    //Finished
    T6 = OFF;
    start = 0;
    stop = 1;
    pc.printf("Program \"Line Follow\" stopped\n\r");
}

////// PROGRAMM REMOTE CONTROL //////////////////////////////////////////

/**
* @brief Control the XBeat via remote
*/
void RemoteControl() {
    Drive XBeat(p22,p21);       //Left, Right Servo Motor
    pc.printf("Program \"Remote Control\" started\n\r");
    XBeat.setoffset(OFFSET_L,OFFSET_R);
    pc.printf("Currently not implemented.\n\r");
    start = 0;
    stop = 1;
    pc.printf("Program \"Remote Control\" stopped\n\r");
}


////// PROGRAMM MAZE SOLVER /////////////////////////////////////////////

#define MAZE_SPEED 60
#define MAZEDUTYCYCLE 0.01
/**
 * @brief drives the XBeat thru a maze
 * made for 27C3 Maze Logo
 */
void MazeSolver() {
    Drive XBeat(p22,p21);       //Left, Right Servo Motor
    pc.printf("Program \"Maze Solver\" started\n\r");
    XBeat.setoffset(OFFSET_L,OFFSET_R);
    pc.printf("The Implementation is currently not finished. I working on it and release this later.\n\r");
    start = 0;
    stop = 1;
    pc.printf("Program \"Maze Solver\" stopped\n\r");
}

///// PROGRAMM DRIVE AND TURN /////////////////////////////////////////////

void driver() {
    SRF05 srf(p6, p5);          // Front Sensor
    Drive XBeat(p21,p22);       // Left, Right Servo Motor
    signed int m1=0,m2=0;       // Motor Set values
    float front=0, front_old=0; // measured value from front sensor
    int turn=0;                 // flag for indicate a turn
    int ticks=0;                // loop counter for turn

    pc.printf("\n\rJust Moving\n\r\n\r");
    XBeat.setoffset(OFFSET_L,OFFSET_R);

    while (!stop) {
        if (!turn) {
            if ((front < front_old) && (front > 8.0)) { //close to a front wall, slow stopping
                if (m1 < 0) m1 = m1-1;
                if (m2 < 0) m2 = m2-1;
                pc.printf("1\n\r");
            } else {
                if (m1 < 100) m1++;
                if (m2 < 100) m2++;
            }
            front_old = front;
        }

        if (turn) {
            ticks++;
            if (ticks <= 300) {
                if (m1 < 50) m1++;
                if (m2 > -50) m2--;
            } else {
                if (m1 >= 0) m1--;
                if (m2 <= 0) m2++;
                if (m1 == 0 || m2 == 0) {
                    turn=0;
                    ticks=0;
                }
            }
        }

        if (front < 8.0 && !turn) {
            m1=0;
            m2=0;
            turn=1;
            ticks=0;
        }
        XBeat.move(m1,m2);
    }
    start = 0;
    stop = 1;
    pc.printf("Program \"Driving\" stopped\n\r");
}


/////// Sensor Data ///////////////////////////////////////////////////////

/**
*   @brief Writing Sensor Data to UART, for debugging purpose
*/
void SensorData() {
    SRF05 srf(p6, p5);
    T6 = ON;
    T7 = ON;
    pc.printf("Sensor Data\n\r\n\r");
    while (!stop) {
        pc.printf("IR-Left %d, IR-Right %d, SRF05 %f\n\r", Left.read_u16()/100, Right.read_u16()/100, srf.read());
        pc.printf("IRL %d, IRM %d, IRR %d\n\r", IRL.read_u16()/100, IRM.read_u16()/100, IRR.read_u16()/100);
        pc.printf("LiPo Voltage %f\n\r\n\r", getLiPoVoltage() );
        wait (1);
    }
    T7 = OFF;
    T6 = OFF;
    start = 0;
    stop = 1;
}


/////// RECALLED FUNCTIONS ////////////////////////////////////
/**
 * @brief function called ervery 1ms to read the button states
 */
void checkButtons () {
    //int c=0;
    if (SW1) {
        SW1_state = 1;
    } else SW1_state = 0;
    if (SW1_old == 1 && SW1_state == 0) {       //falling edge
        /** Add Code to do when Button 1 is pressed */
        start = !start;
        stop = !start;
    }
    SW1_old = SW1_state;

    if (SW2) SW2_state = 1;
    else SW2_state = 0;
    if (SW2_old == 1 && SW2_state == 0) {       //falling edge
        /** Add Code to do when Button 2 is pressed */
        if (stop) {
            if (programm_counter < 5) programm_counter++;
            else programm_counter = 1;

            switch (programm_counter) {
                case 1:
                    pc.printf("Programm %d:  Line Follow\n\r", programm_counter );
                    break;
                case 2:
                    pc.printf("Programm %d:  Maze Solver\n\r", programm_counter );
                    break;
                case 3:
                    pc.printf("Programm %d:  Driving\n\r", programm_counter );
                    break;
                case 4:
                    pc.printf("Programm %d:  Remote Control\n\r", programm_counter );
                    break;
                case 5:
                    pc.printf("Programm %d:  Sensor Data\n\r", programm_counter );
                    break;
            }
            if (programm_counter == 5) {
                led1=1;
                led4=1;
            } else    setLed(programm_counter);
        }
    }
    SW2_old = SW2_state;
}



/** @brief calculates the battery voltage
*   @return real voltage als float */
float getLiPoVoltage() {
    return ((ubat*UBAT_R_FACTOR*3.3)+0.79);
}

/**@brief function called every 1s to check LiPo Voltage */
void LiPo_Warning() {
    int c = 0;
    //LiPo Undervoltage Emergency Stop
    if ((getLiPoVoltage()*10) < UBAT_LOW) {

        start=0;
        stop=1;
        if (firstUnderVoltage) { //need to return 1st time to stop the motors
            firstUnderVoltage = 0;
            return;
        } else {
            while (1) { //stay here
                Speaker = 1;
                led4 = 1;
                wait(0.1);
                led4 = 0;
                Speaker = 0;
                wait(1);
                if (c++ == 5) {
                    pc.printf("LiPo Voltage: %fV\n\r",getLiPoVoltage());
                    c=0;
                }
            }
        }
    }
}

//////// HELPER ///////////////////////////////////////////////////////////
/** @ brief set the LEDs on the mbed dev board
 * @parameter led - LED Number (1-4)
 */
void setLed(int led) {
    led1=OFF;
    led2=OFF;
    led3=OFF;
    led4=OFF;

    switch (led) {
        case 1:
            led1 = ON;
            break;
        case 2:
            led2 = ON;
            break;
        case 3:
            led3 = ON;
            break;
        case 4:
            led4 = ON;
            break;
    }
}




/** @brief Initializations */
void init() {
    pc.baud(BAUDRATE);                          //set uart baudrate
    tick_1ms.attach_us(&checkButtons, 1000);    //ervery 1ms
    firstUnderVoltage = 1;                      // set flag
    LiPoTicker.attach(&LiPo_Warning,1);         //check LiPo voltage every second
}

///////// MAIN  ///////////////////////////////////////////////////////////

/**
*   @brief main function
*/
int main() {
    init();
    setLed(programm_counter);
    pc.printf("---- XBeat ----\n\r\n\r");
    pc.printf("Initial Program 1: Line Follow\n\r");
    while (1) {
        wait(0.1);
        if (start) {
            switch (programm_counter) {
                case 1:
                    LineFollow();
                    setLed(programm_counter);
                    break;
                case 2:
                    MazeSolver();
                    setLed(programm_counter);
                    break;
                case 3:
                    driver();
                    setLed(programm_counter);
                    break;
                case 4:
                    RemoteControl();
                    setLed(programm_counter);
                    break;
                case 5:
                    SensorData();
                    setLed(programm_counter++);
                    break;
                default:
                    break;
            }
        }
    }
}