File content as of revision 39:a4fd6206da89:

/*! \mainpage Index Page
 * @author Christian Burri
 * @author Arno Galliker
 *
 * @copyright Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
 * All rights reserved.
 *
 * @brief
 *
 * This program is for an autonomous robot for the competition
 * at the Hochschule Luzern.
 * We are one of the 32 teams. In the team #1 is:
 * - Bauernfeind Julia <B>WI</B>
 * - Büttler Pirmin <B>WI</B>
 * - Amberg Reto <B>I</B>
 * - Galliker Arno <B>I</B>
 * - Amrein Marcel <B>M</B>
 * - Flühler Ramon <B>M</B>
 * - Burri Christian <B>ET</B>
 *   Or see: <a href="http://cruisingcrepe.wordpress.com/">http://cruisingcrepe.wordpress.com/</a>
 *
 * The postition control is based on this Documentation: Control of Wheeled Mobile Robots:
 * An Experimental Overview from Alessandro De Luca, Giuseppe Oriolo, Marilena Vendittelli.
 * For more information see here: <a href="http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf">http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf</a>
 *
 * The connection to an android smartphone is realise with the library MicroBridge(Android ADB) from Junichi Katsu.
 * For more information see here: <a href="http://mbed.org/users/jksoft/code/MicroBridge/">http://mbed.org/users/jksoft/code/MicroBridge/</a>
 *
 * The rest of the classes are only based on standard library from mbed.
 * For more information see here: <a href="http://mbed.org/users/mbed_official/code/mbed/">http://mbed.org/users/mbed_official/code/mbed/</a>
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * @file main.cpp
 */

#include "defines.h"
#include "State.h"
#include "RobotControl.h"
#include "androidADB.h"

PwmOut led[4] = {LED1, LED2, LED3, LED4};

/**
 * @name Hallsensor
 * @{
 */

/**
* @brief <code>hallsensorLeft</code> object with pin15, pin17 and pin16
*/
Hallsensor hallLeft(p18, p17, p16);

/**
 * @brief <code>hallsensorLeft</code> object with pin17, pin28 and pin29
 */
Hallsensor hallRight(p27, p28, p29);
/*! @} */

/**
 * @name Motors and Robot Control
 * @{
 */

/**
* @brief <code>leftMotor</code> object with pin26, pin25, pin24,
* pin19 and <code>hallsensorLeft</code> object
*/
MaxonESCON leftMotor(p26, p25, p24, p19, &hallLeft);

/**
 * @brief <code>rightMotor</code> object with pin23, pin22, pin21,
 * pin20 and <code>hallsensorRight</code> object
 */
MaxonESCON rightMotor(p23, p22, p21, p20, &hallRight);

/**
 * @brief <code>robotControl</code> object with <code>leftMotor</code>,
 * <code>rightMotor</code> and the sampling rate for the run method
 */
RobotControl robotControl(&leftMotor, &rightMotor, PERIOD_ROBOTCONTROL);
/*! @} */

/**
 * @name Logging & State
 * @{
 */

/**
 * @brief Define the struct for the State and the Logging
 */
state_t s;

/**
 * @brief <code>state</code> object with <code>robotControl</code>,
 * <code>rightMotor</code>, <code>leftMotor</code>, <code>battery</code>
 * and the sampling rate for the run method
 */
State state(&s, &robotControl, &leftMotor, &rightMotor, p15, PERIOD_STATE);
/*! @} */

/**
 * @name Communication
 * @{
 */

/*! @} */

/**
* @brief Main function. Start the Programm here.
*/
int main()
{

    /**
     * Check at first the Battery voltage. Starts when the voltage
     * is greater than the min is.
     * start the timer for the Logging to the file
     * and start the Task for logging.
     **/
    state.start();

    while(s.voltageBattery < BAT_MIN) {
        for (float f = 0.1f; f < 6.3f; f += 0.1f) {
            for(int i = 0; i <= 3; i  ++) {
                led[i] = state.dim( i, f );
            }
            wait_ms(20);
        }
        wait(0.05);
        for (float f = 0.1f; f < 6.3f; f += 0.1f) {
            for(int i = 0; i <= 3; i  ++) {
                led[i] = state.dim( 3-i, f );
            }
            wait_ms(20);
        }
        wait(0.05);
    }
    for(int i = 0; i <= 3; i  ++) {
        led[i] = state.dim( 0, 0.0f );
    }

    wait(0.5);
    state.initPlotFile();
    state.startTimerFromZero();
    robotControl.start();
    /*
        /**
         * Clear all Errors of the ESCON Module, with a disabled to enable event.
         */
    robotControl.setEnable(false);
    wait(0.01);
    robotControl.setEnable(true);
    wait(0.1);
    robotControl.setEnable(false);

    /**
    * Initialise the ADB subsystem.
    * Set Theta for to go to Startloop.
    */
    init();
    setDesiredTheta(400.0f);

    while(getDesiredTheta() < (4 * PI)) {

        if (getDesiredTheta() > (2 * PI)) {
            /**
            * Runs at first till the Startbutton has pressed.
            */
            
            state.startTimerFromZero();
            robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
            robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
            wait(PERIOD_ANDROID/2);
        } else {
            robotControl.setEnable(true);
            robotControl.setDesiredPositionAndAngle(getDesiredX(), getDesiredY(), getDesiredTheta());
            state.savePlotFile(s);
            wait(PERIOD_ANDROID/2);
            state.savePlotFile(s);
        }
        writeActualPosition(robotControl.getxActualPosition(),
                            robotControl.getyActualPosition(),
                            robotControl.getActualTheta(),
                            s.state&STATE_UNDER,
                            s.state&STATE_LEFT,
                            s.state&STATE_RIGHT,
                            s.voltageBattery);
        wait(PERIOD_ANDROID/2);
        
        ADB::poll();
    }

    /**
     * Sets the acutal value for a fast stop.
     * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm.
     */
    robotControl.setDesiredPositionAndAngle(robotControl.getxActualPosition(),
                                            robotControl.getyActualPosition(),
                                            robotControl.getActualTheta());
    robotControl.stop();
    leftMotor.setVelocity(0.0f);
    rightMotor.setVelocity(0.0f);
    wait(2);

    /**
     * Close the File PLOTS.txt to read the file with the computer afterwards and draw a diagramm.
     */
    state.closePlotFile();
    state.stop();
    robotControl.setEnable(false);

    /**
    * Fast PWM sample for the end.
    */
    while(1) {
        for (float f = 0.1f; f < 6.3f; f += 0.1f) {
            for(int i = 0; i <= 3; i  ++) {
                led[i] = state.dim( i, f );
            }
            wait_ms(5);
        }
        wait(0.1);
        for (float f = 0.1f; f < 6.3f; f += 0.1f) {
            for(int i = 0; i <= 3; i  ++) {
                led[i] = state.dim( 3-i, f );
            }
            wait_ms(5);
        }
        wait(0.05);
    }
}