Revision 2:d8e1613dc38b, committed 2013-03-03

Comitter:

chrigelburri

Date:

Sun Mar 03 16:26:47 2013 +0000

Parent:

1:6cd533a712c6

Child:

3:92ba0254af87

Commit message:

Viereck Fahren; Code aufger?umt und eine setter methode progammiert f?r sollwert

Changed in this revision

--- a/Actuators/MaxonESCON/MaxonESCON.cpp	Sat Mar 02 09:39:34 2013 +0000
+++ b/Actuators/MaxonESCON/MaxonESCON.cpp	Sun Mar 03 16:26:47 2013 +0000
@@ -27,7 +27,7 @@
 
     // Set the pulses to zero
     _pulses = 0;
-    
+
     // Set the Pull Up Resistor
     _isenb.mode(PullUp);
 }
@@ -66,10 +66,10 @@
 bool MaxonESCON::isEnabled()
 {
     if(_isenb.read() == 1) {
-         return true;
-     } else {
-         return false;
-     }
+        return true;
+    } else {
+        return false;
+    }
 }
 
 int MaxonESCON::getPulses(void)
@@ -84,45 +84,3 @@
     _pulses = _hall->getPulses();
     return _pulses;
 }
-
-
-///// Für Polling muss mit unter Code gearbeitet werden.
-
-/*
-int MaxonESCON::getHallPosition(void)
-{
-    return HALL_POSITION[(_hall1.read() ? 1 : 0)+(_hall2.read() ? 2 : 0)+(_hall3.read() ? 4 : 0)];
-}
-
-int MaxonESCON::getPosition(void)
-{
-     position calculation
-
-int actualHallPosition = getHallPosition();
-
-if (actualHallPosition > 0) {
-    if ((actualHallPosition == 1) && (hallPosition >= 5)) {
-        turns++;
-    } else if ((actualHallPosition == 2) && (hallPosition == 6)) {
-        turns++;
-    } else if ((actualHallPosition == 5) && (hallPosition == 1)) {
-        turns--;
-    } else if ((actualHallPosition == 6) && (hallPosition <= 2)) {
-        turns--;
-    }
-    hallPosition = actualHallPosition;
-}
-return turns;
-}
-
-float MaxonESCON::getTransPosition(void)
-{
-return (turns * 2 * _wheelRadius * pi) / (_gear * _polePairs); // PULSES_PER_STEP muss weg da nach 6 hallimpus ein turn gibt
-}
-
-int MaxonESCON::setPosition(int setPos)
-{
-turns = setPos;
-return turns;
-}
-*/

--- a/Actuators/MaxonESCON/MaxonESCON.h	Sat Mar 02 09:39:34 2013 +0000
+++ b/Actuators/MaxonESCON/MaxonESCON.h	Sun Mar 03 16:26:47 2013 +0000
@@ -24,21 +24,18 @@
 class MaxonESCON
 {
 
-protected:
+private:
 
+    /** To Enable the amplifier */
+    DigitalOut _enb;
     /** Duty Cycle to set the speed */
     PwmOut _pwm;
-    /** To Enable the amplifier */
-    DigitalOut _enb;
     /** Hallsensor Class */
     Hallsensor* _hall;
     /** Ready output from ESCON */
     DigitalIn _isenb;
     /** Actual speed from ESCON analog Output 1 */
     AnalogIn _actualSpeed;
-
-private:
-
     /** increment the Hallpattern */
     int    _pulses;
 

--- a/Android/Android.cpp	Sat Mar 02 09:39:34 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,201 +0,0 @@
-#include "Android.h"
-
-using namespace std;
-
-
-Android::Android(state_t* s, RobotControl* robotControl, MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, float period)
-    :
-    Task(period),
-    AndroidAccessory (OUTL, INBL, MANUFACTURER, MODEL, DESCRIPTION, VERSION, URI, SERIAL)
-{
-    /* get peripherals */
-    this->s = s;
-    this->robotControl = robotControl;
-    this->motorControllerLeft = motorControllerLeft;
-    this->motorControllerRight = motorControllerRight;
-    this->period = period;
-
-    setupDevice();
-    USBInit();
-
-}
-
-
-Android::~Android() {}
-
-
-void Android::run()
-{
-
-    USBLoop();
-
-
-    /*
-
-          SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN
-
-          HIer die Sollwertvorgabe position und Winkel
-          robotContol->setxPosition(1.0f);
-          robotContol->setyPosition(1.0f);
-          robotContol->setTheta(1/2 * piiii);
-
-
-          SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN SENDEN
-
-
-
-
-          LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN
-
-          Hier die aktuelle Position und Winkel
-          s->xAxis oder robotControl->getxActualPosition();
-          s->yAxis oder robotControl->getyActualPosition();
-          s->angle = robotControl->getActualTheta() * 180 / PI;
-
-
-          Batterie
-          s->voltageBattery
-
-          Status
-          s->state;
-
-          LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN LESEN
-
-
-
-
-
-          BEim Start muss das Kommen nach dem Start Klicken muss alles auf null gesetzt werden. und los gehts
-
-          compass.calibrate_compass_basis_rad();
-
-          robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET);
-          leftMotor->setPulses(0);
-          rightMotor->setPulses(0);
-          robotControl->setEnable(false);
-          wait(0.2);
-          robotControl->setEnable(true);
-          wait(0.2);
-          state->startTimerFromZero();
-          state->initPlotFile();
-          state->start();
-
-          ENDE Start
-
-
-
-          Beim ENDE Muss das kommen
-                  state->savePlotFile(s);
-          ENDE
-
-      */
-}
-
-int Android::callbackRead(u8 *buf, int len)
-{
-    // pc.printf("%i  %s\n\r\n\n\n",len,buf);
-    for (int i = 0; i<INBL; i++) {
-        buf[i] = 0;
-    }
-}
-
-
-void Android::setupDevice()
-{
-    //  pc.baud(460800);
-    //  pc.printf("Welcome to Android\n\n\n\n\n\n\r");
-    settick = false;
-    //  pc.attach(this, &Android::serialIRQ, Serial::RxIrq);
-    for (int i = 0; i<OUTL; i++) {
-        buffer[i] = 0;
-    }
-    bcount = 0;
-    //n.attach(this,&Android::AttachTick,5);
-    //tick.attach(this,&Android::onTick,0.1);
-}
-void Android::resetDevice()
-{
-    //pc.printf("Android reset\n\r");
-    for (int i = 0; i<OUTL; i++) {
-        buffer[i] = 0;
-    }
-    bcount = 0;
-}
-
-int Android::callbackWrite()
-{
-    return 0;
-}
-
-
-void Android::AttachTick()
-{
-    if(!settick)tick.attach(this,&Android::onTick,0.04);
-    settick = true;
-}
-
-void Android::onTick()
-{
-    bool update = false;
-    int templ, tempr;
-
-
-
-    templ = int(left * 10000);
-    tempr = int(right * 10000);
-
-
-
-    if (abs(templ-tl)>170) {
-        update = true;
-    }
-    if (abs(tempr-tr)>170) {
-        update = true;
-    }
-    if (update) {
-        u8* wbuf = _writebuff;
-
-        wbuf[0] = 'P';
-        wbuf[1] =  templ&0xFF;
-        wbuf[2] = (templ>>8) & 0xFF;
-        wbuf[3] =  tempr&0xFF;
-        wbuf[4] = (tempr>>8) & 0xFF;
-        wbuf[5] = 0;
-
-        this->write(wbuf,5);
-
-    }
-}
-
-
-void Android::serialIRQ()
-{
-    //  buffer[bcount] = pc.getc();
-
-    // pc.putc(buffer[bcount]);
-
-    if (buffer[bcount] == '\n' || buffer[bcount] == '\r') {
-        u8* wbuf = _writebuff;
-        for (int i = 0; i<OUTL; i++) {
-            wbuf[i] = buffer[i];
-            buffer[i] = 0;
-        }
-        //pc.printf("Sending: %s\n\r",wbuf);
-        this->write(wbuf,bcount);
-        bcount = 0;
-    } else {
-        if (buffer[bcount] != 0x08 && buffer[bcount] != 0x7F ) {
-            bcount++;
-            if (bcount == OUTL) {
-                bcount = 0;
-            }
-        } else {
-            bcount--;
-        }
-    }
-
-}
-
-
-
-

--- a/Android/Android.h	Sat Mar 02 09:39:34 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,122 +0,0 @@
-#ifndef _Android_H_
-#define _Android_H_
-
-#include <cmath>
-#include "mbed.h"
-#include "RobotControl.h"
-#include "AndroidAccessory.h"
-#include "Task.h"
-#include "defines.h"
-
-// Communication
-#define OUTL             100                   //The size of the read buffer
-#define INBL             100                   //The size of the write buffer 
-#define MANUFACTURER          "ARM"                 //The manufacturer of the accessory
-#define MODEL                 "mbed"                //The model of the accessory
-#define DESCRIPTION           "mbed Terminal"       //A short description of the accessory
-#define VERSION               "0.1"                 //The current version of the accessory
-#define URI                   "http://www.mbed.org" //Some data to go with the accessory (URL or more description)
-#define SERIAL                "0000000012345678"    //The serial number of the accessory 
-
-
-/**
- * @author Christian Burri
- *
- * @section LICENSE
- *
- * Copyright (c) 2013 HSLU Pren Team #1 Cruising Crêpe
- * All rights reserved.
- *
- * @section DESCRIPTION
- * IMMPORTANT: OnLoadDevice(..) muss noch bei AndroidAccesory.cpp auskommentiert werden, weil es schon beim BLUE USB vorkommt.
- * AUCH WICHTIG: Nach dem Löschen von BLUE USB Muss der USBHost neu importiert werden im Andoid/AndroidAccessory Es sind drei Dateinen
- * Connect to an Android Smartphone......
- */
-
-class Android : public AndroidAccessory, Task
-{
-
-private:
-
-    state_t*            s;
-    RobotControl*       robotControl;
-    MaxonESCON*         motorControllerLeft;
-    MaxonESCON*         motorControllerRight;
-    float               period;
-
-public:
-
-
-    /**
-     * Creates a Android object and initializes all private
-     * Android variables.
-     * @param RobotControl Objekt to read the State.
-     * @param motorControllerLeft a reference to the servo drive for the left motor.
-     * @param motorControllerRight a reference to the servo drive for the right motor.
-     * @param period the sampling period of the run loop of this controller, given in [s].
-     */
-    Android(state_t* s, RobotControl* robotControl,MaxonESCON* motorControllerLeft, MaxonESCON* motorControllerRight, float period);
-
-    /**
-    * Destructor of the Object to destroy the Object.
-    **/
-    virtual     ~Android();
-
-
-    /** Init the device
-    * This is meant to be implimented by the user of the class
-    *
-    * @param device Device number
-    * @param configuration Configuration
-    * @param interfaceNumber Inteface number
-    */
-  /////////////////////////////  virtual void    init (int device, int configuration, int interfaceNumber);
-
-    /** Reset the device
-    * This is meant to be implimented by the user of the class
-    *
-    */
-    virtual void    resetDevice ();
-
-    /** Setup the device
-    * This is meant to be implimented by the user of the class. Called when the device is first intialised
-    *
-    */
-    virtual void    setupDevice ();
-
-    /** Callback on Read
-    * This is meant to be implimented by the user of the class. Called when some data has been read in.
-    *
-    * @param buff The buffered read in data
-    * @param len The length of the packet recived
-    *
-    */
-    virtual int     callbackRead (u8 *buff, int len);
-
-    /** Callback after Write
-    * This is meant to be implimented by the user of the class. Called when the write has been finished.
-    *
-    */
-    virtual int     callbackWrite ();
-
-
-    void        run();
-
-
-private:
-
-    void serialIRQ();
-    void onTick();
-    void AttachTick();
-    char buffer[INBL];
-    int bcount;
-    Ticker tick;
-    float right,left,rl,ll;
-    int tl,tr;
-    Timeout n;
-    bool settick;
-
-
-};
-
-#endif

--- a/Android/AndroidAccessory.lib	Sat Mar 02 09:39:34 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/p07gbar/code/AndroidAccessory/#b691d42306d9

--- a/RobotControl/RobotControl.cpp	Sat Mar 02 09:39:34 2013 +0000
+++ b/RobotControl/RobotControl.cpp	Sun Mar 03 16:26:47 2013 +0000
@@ -23,7 +23,6 @@
 
     Desired.setAcceleration(ACCELERATION);
     Desired.setThetaAcceleration(THETA_ACCELERATION);
-
 }
 
 RobotControl::~RobotControl()
@@ -77,6 +76,13 @@
     Desired.theta = theta;
 }
 
+void RobotControl::setPositionAngle(float xposition, float yposition, float theta)
+{
+    setxPosition(xposition);
+    setyPosition(yposition);
+    setTheta(theta);
+}
+
 float RobotControl::getTheta()
 {
     return Desired.theta;
@@ -139,12 +145,32 @@
 
 float RobotControl::getThetaErrorToGoal()
 {
-    return atan2(getyPositionError(),getxPositionError()) - getActualTheta(); // PI ist weg weil auch negative Zahlen zugelassen werden.
+    float temp;
+    temp = atan2(getyPositionError(),getxPositionError()) - getActualTheta();
+
+    if(temp <= -PI) {
+        temp  += 2*  PI;
+    } else if (temp > PI) {
+        temp  -= 2*  PI;
+    } else {
+        //nothing
+    }
+    return temp;
 }
 
 float RobotControl::getThetaGoal()
 {
-    return atan2(getyPositionError(),getxPositionError()) - getTheta();
+    float temp;
+    temp = atan2(getyPositionError(),getxPositionError()) - getTheta();
+
+    if(temp <= -PI) {
+        temp  += 2*  PI;
+    } else if (temp > PI) {
+        temp  -= 2*  PI;
+    } else {
+        //nothing
+    }
+    return temp;
 }
 
 void RobotControl::setAllToZero(float xZeroPos, float yZeroPos, float theta)
@@ -157,17 +183,16 @@
     omega = 0.0f;
 }
 
-
 void RobotControl::run()
 {
     ///// DAs kan glaub raus ab hier
-
+/////////////////////////////////////////////////////////7
     /* motion planning */
     if (isEnabled()) {
         ///// DAs kan glaub raus bis hier
         Desired.increment(speed, omega, period);
 
-        ///// DAs kan glaub raus vis hier
+        ///// DAs kan glaub raus bis hier
 
     } else {
         speed = 0.0f;
@@ -201,19 +226,19 @@
     Actual.xposition += (Actual.speed * period * cos(Actual.theta));
     Actual.yposition += (Actual.speed * period * sin(Actual.theta));
 
-
     //  Actual.thetaCompass = compass->getFilteredAngle();
     /* translational X and Y Position. integrate the speed with the time theta from compass */
     //  Actual.xposition += - (Actual.speed * period * cos(Actual.thetaCompass));
     //  Actual.yposition += (Actual.speed * period * sin(Actual.thetaCompass));
-    
+
     /* motor control */
     if ( isEnabled() &&  ( getDistanceError() >= MIN_DISTANCE_ERROR ) )  {
 
         /* postition control */
-         
+
         speed = K1 * getDistanceError() * cos( getThetaErrorToGoal() );
-        omega = K2 * getThetaErrorToGoal() + K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) / (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() );
+        omega = K2 * getThetaErrorToGoal() +
+                K1 * ( ( sin(getThetaErrorToGoal()) * cos(getThetaErrorToGoal()) ) / (getThetaErrorToGoal()) ) * ( getThetaErrorToGoal() + K3 * getThetaGoal() );
 
         motorControllerLeft->setVelocity( ( ( (2 * speed) - (WHEEL_DISTANCE * omega) ) / 2 ) / (2 * WHEEL_RADIUS * PI) * GEAR );
         motorControllerRight->setVelocity(-( ( (2 * speed) + (WHEEL_DISTANCE * omega) ) / 2) / (2 * WHEEL_RADIUS * PI) * GEAR );
@@ -225,4 +250,3 @@
 
     }
 }
-

--- a/RobotControl/RobotControl.h	Sat Mar 02 09:39:34 2013 +0000
+++ b/RobotControl/RobotControl.h	Sun Mar 03 16:26:47 2013 +0000
@@ -46,15 +46,6 @@
     float               speed;
     float               omega;
 
-    float               rho; /* Distance to goal [m]*/
-    float               gamma; /* Angle of the start position to goal position [rad]*/
-    float               delta; /* Angle of the goal postition [rad]*/
-
-    float               deltaXPostition; /* differenz of the actual X-value and the desired [m] */
-    float               deltaYPostition; /* differenz of the actual X-value and the desired [m] */
-
-
-
 public:
 
     /**
@@ -110,7 +101,7 @@
     void        setDesiredOmega(float omega);
 
     /**
-    * Sets the desired X- position of the robot.
+    * Sets the desired X-position of the robot.
     * @param xposition the desired position, given in [m].
     */
     void        setxPosition(float xposition);
@@ -122,12 +113,20 @@
     void        setyPosition(float yposition);
 
     /**
-    * Sets the angle of the robot.
+    * Sets the desired angle of the robot.
     * @param theta the desired angle, given in [rad].
     */
     void        setTheta(float theta);
     
     /**
+    * Sets the desired Position and angle.
+    * @param xposition the desired position, given in [m].
+    * @param yposition the desired position, given in [m]. 
+    * @param theta the desired angle, given in [rad].  
+    */
+    void        setPositionAngle(float xposition, float yposition, float theta);
+
+    /**
      * Gets the desired Theta. Angle of the goal Point.
      * @return the desired Theta, given in [rad].
      */
@@ -191,7 +190,6 @@
     * Gets the orientation following error of the robot.
     * @return the orientation following error, given in [rad].
     */
-    
     float       getThetaError();
     
     /**
@@ -212,7 +210,6 @@
     */
     float       getThetaGoal();
 
-
     /**
     * Set all state to zero
     * @param xZeroPos Sets the start X-position [m].

--- a/StateDefines/State.cpp	Sat Mar 02 09:39:34 2013 +0000
+++ b/StateDefines/State.cpp	Sun Mar 03 16:26:47 2013 +0000
@@ -15,13 +15,11 @@
     this->robotControl = robotControl;
     this->motorControllerLeft = motorControllerLeft;
     this->motorControllerRight = motorControllerRight;
-   // this->compass = compass;
+    // this->compass = compass;
     this->battery =battery;
     this->period = period;
-
 }
 
-
 State::~State() {}
 
 void State::initPlotFile(void)
@@ -50,9 +48,12 @@
                 "SetpointAngel[grad]\t"
                 "RightDistanceSensor[m]\t"
                 "CompassAngle[grad]\t"
-                "CompassX-Axis\t"
+                "CompassX-Axis\t" //20
                 "CompassY-Axis\t"
-                "State\n");
+                "State\t"
+                "distanceToGoal[m]\t" //23
+                "angleToGoal[rad]\t"
+                "thetaFromTheGoal[rad]\n");
     }
 }
 
@@ -61,7 +62,7 @@
 {
     char buf[256];
 
-    sprintf(buf,"%d\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d",
+    sprintf(buf,"%d\t%f\t%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%d\t%f\t%f\t%f",
             s.millis,
             s.voltageBattery,
             s.leftPulses,
@@ -83,13 +84,15 @@
             s.compassAngle,
             s.compassxAxis,
             s.compassyAxis,
-            s.state
+            s.state,
+            s.rho,
+            s.lamda,
+            s.delta
            );
 
     if (logp)
         fprintf(logp, buf);
     fprintf(logp, "\n"); // new line
-
 }
 
 void State::savePlotText(char text[])
@@ -97,11 +100,9 @@
     fprintf(logp, text);
 }
 
-
 void State::closePlotFile(void)
 {
     fclose(logp);
-
 }
 
 float State::readBattery()
@@ -142,7 +143,6 @@
     timer.start();
 }
 
-
 void State::run()
 {
     s->millis = timer.read_ms();
@@ -159,8 +159,11 @@
     s->xAxisError = robotControl->getxPositionError();
     s->yAxisError = robotControl->getyPositionError();
     s->angleError = robotControl->getThetaError() * 180 / PI;
-   // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
+    // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
 
+    s->rho = robotControl->getDistanceError();
+    s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI;
+    s->delta = robotControl->getThetaGoal() * 180 / PI;
 
     setBatteryBit();
     setEnableLeftBit();

--- a/StateDefines/State.h	Sat Mar 02 09:39:34 2013 +0000
+++ b/StateDefines/State.h	Sun Mar 03 16:26:47 2013 +0000
@@ -40,8 +40,6 @@
 
     float               magout[3];
 
-
-
 public:
 
 
@@ -61,15 +59,12 @@
     * Destructor of the Object to destroy the Object.
     **/
     virtual     ~State();
-
-
+    
     /**
      * Initzialize the File. Open the File plots.txt and set the title at first line
      */
     void initPlotFile(void);
 
-
-
     /**
     * Save the char to the file.
     * For example at the end.
@@ -77,13 +72,11 @@
     */
     void savePlotText(char text[]);
 
-
     /**
     * Close the File
     */
     void closePlotFile(void);
 
-
     /**
      * Return the Battery voltage
      * state variables.

--- a/StateDefines/defines.h	Sat Mar 02 09:39:34 2013 +0000
+++ b/StateDefines/defines.h	Sun Mar 03 16:26:47 2013 +0000
@@ -26,7 +26,7 @@
 #define ESCON_GET_FACTOR      100.0f        // Speed Factor how get in the ESCON       
 
 // Start Defintition
-#define START_X_OFFSET        -0.8f          // Sets the start X-point [m]
+#define START_X_OFFSET        -0.8f         // Sets the start X-point [m]
 #define START_Y_OFFSET        0.8f          // Sets the start Y-point [m]
 
 // Maximum Aceeleration
@@ -34,25 +34,23 @@
 #define THETA_ACCELERATION    1.0f          // maximum rotational acceleration, given in [rad/s2]
 
 // Gains of the position controller
-#define GAIN    0.2f
-#define K1    1.0f * GAIN 
-#define K2    3.0f * GAIN  
-#define K3    2.0f * GAIN
+#define GAIN                  0.30f         // Main Gain
+#define K1                    1.0f * GAIN
+#define K2                    3.0f * GAIN
+#define K3                    2.0f * GAIN
 
-#define MIN_DISTANCE_ERROR 0.1 // min. Distance to switch the position controller off. Because when Distance Error goes to zero the ATAN2 is not define, given in [m]         
+#define MIN_DISTANCE_ERROR 0.03             // min. Distance to switch the position controller off. Because when Distance Error goes to zero the ATAN2 is not define, given in [m]                 
 
 // LiPo Batterie
-#define BAT_MULTIPLICATOR     21.633333333f // R2 / (R1 + R2) = 0.153    R2= 10k , R1 = 1.8k  
-// 1/0.153 = 6.555  ---> 3.3/1 * 6.555 = 21.6333333f
-#define BAT_MIN               17.5f         // minium operate voltage [V] Battery Type: 1SP1P LG-18650 -->
-// nominal voltage 3.6V   --> 5 batterys ==> 5 * 3.5V = 17.5V
+#define BAT_MULTIPLICATOR     21.633333333f // R2 / (R1 + R2) = 0.153    R2= 10k , R1 = 1.8k 1/0.153 = 6.555  ---> 3.3 * 6.555 = 21.6333333f
+#define BAT_MIN               17.5f         // minium operate voltage [V] Battery Type: 1SP1P LG-18650 --> nominal voltage 3.6V   --> 5 batterys ==> 5 * 3.5V = 17.5V
+
 // Frequenz for the Task
 #define PERIOD_COMPASS        0.050f        // 20Hz Rate for Compass HMC6352
 #define PERIOD_ROBOTCONTROL   0.001f        // 1kHz Rate for Robot Control
 #define PERIOD_STATE          0.001f        // 1kHz Rate for State Objekt 
 #define PERIOD_ANDROID        0.1f          // 10Hz Rate for State Objekt 
 
-
 // Compass Maxima und Minima for the Filter
 #define SET_MAXIMAS_MINIMAS   true          // For Set the maximas und minimas when false the object search the maximas minimas by your own
 /*#define COMP_X_MAX            344.464996f   // Maximum X-Range
@@ -69,16 +67,10 @@
 #define COMP_Y_MIN            -247.647675f  // Minimum Y-Range      
 #define COMP_Z_MIN            -385.915009f  // Minimun Z-Range not used in this side  
 
-
-/**
-* struct state
-   * structure containing system sensor data
-   ****** System Status
-   ****** Data reported Motor
-   ****** Data reported Car
-   ****** Set Point Car
-   ****** measuring Position and angle
-   **/
+    /**
+    * struct state
+    * structure containing system sensor data
+    **/
 typedef struct state {
     /** millis Time [ms]*/
     int millis;
@@ -124,6 +116,12 @@
     float compassyAxis;
     /** State of the Car **/
     int state;
+    /** distance to Goal */
+    float rho;
+    /** theta to goal */
+    float lamda;
+    /** theta from the goal */
+    float delta;
 } state_t;
 
 

--- a/main.cpp	Sat Mar 02 09:39:34 2013 +0000
+++ b/main.cpp	Sun Mar 03 16:26:47 2013 +0000
@@ -1,12 +1,17 @@
-/*
- * main.cpp
- * Copyright (c) 2012, Pren Team1 HSLU T&A
+/**
+ * @author Christian Burri
+ *
+ * @section LICENSE
+ *
+ * Copyright © 2013 HSLU Pren Team #1 Cruising Crêpe
  * All rights reserved.
+ *
+ * @section DESCRIPTION
+ *
+ * ?????
+ *
  */
 
-///////////////////////////////////////////////////////////////////////////////////////////////////////
-// INCLUDES
-///////////////////////////////////////////////////////////////////////////////////////////////////////
 #include "mbed.h"
 #include "math.h"
 #include "defines.h"
@@ -16,8 +21,6 @@
 #include "RobotControl.h"
 #include "Ping.h"
 #include "PowerControl/EthernetPowerControl.h"
-#include "Android.h"
-
 
 // LiPo Batterie
 AnalogIn battery(p15);           // Battery check
@@ -26,10 +29,6 @@
 //HMC5883L compass(p9, p10, PERIOD_COMPASS);       // sda, sdl (I2C)
 //HMC6352 compass(p9, p10, PERIOD_COMPASS);        // sda, sdl (I2C)
 
-
-// ultrasonic sensor
-//Ping ultrasonic(p30);
-
 //Hallsensor
 //hall1, hall2, hall3
 Hallsensor hallLeft(p18, p17, p16);
@@ -49,10 +48,6 @@
 state_t s; // stuct state
 State state(&s, &robotControl, &leftMotor, &rightMotor, /*&compass,*/ &battery, PERIOD_STATE);
 
-// Communication
-
-//Android android(&s, &robotControl, &leftMotor, &rightMotor, PERIOD_ANDROID);
-
 // PC USB communications
 Serial pc(USBTX, USBRX);
 
@@ -65,7 +60,6 @@
 
 int main()
 {
-    pc.printf("blabal");
     /** Normal mbed power level for this setup is around 690mW
     * assuming 5V used on Vin pin
     * If you don't need networking...
@@ -73,42 +67,48 @@
     * Also need to unplug network cable - just a cable sucks power
     */
     PHY_PowerDown();
-    state.startTimerFromZero();
-    state.initPlotFile();
-    
-          //  robotControl.start();
+
+    //  robotControl.start();
     //  compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
     //  compass.start();
+    
+    state.initPlotFile();
+
     robotControl.start();
     robotControl.setEnable(false);
     wait(0.01);
     robotControl.setEnable(true);
     wait(0.01);
     robotControl.setAllToZero(0, 0, PI/2 );
+
     leftMotor.setPulses(0);
     rightMotor.setPulses(0);
+
+    state.startTimerFromZero();
     state.start();
-    pc.printf("start");
-    
-    robotControl.setxPosition(-1.0);
-    robotControl.setyPosition(1.0);
-    robotControl.setTheta( PI);
+
+    robotControl.setPositionAngle(0.0f, 1.0f,  17*PI/18);
     while(!(s.millis >= 15000)) {
         state.savePlotFile(s);
-        pc.printf("rhho: %f, gamma: %f, delta: %f thetaacutal: %f      %f   %f  atan2: %f\n", robotControl.getDistanceError(), robotControl.getThetaErrorToGoal() * 180/PI, robotControl.getThetaGoal()*180/PI, robotControl.getActualTheta()*180/PI, robotControl.getyPositionError(), robotControl.getxPositionError(), atan2(robotControl.getyPositionError(),robotControl.getxPositionError()) * 180/PI);
     };
-    pc.printf("nextpoint");
-    robotControl.setxPosition(-2.0);
-    robotControl.setyPosition(3.0);
-    robotControl.setTheta( PI/2 );
+
+    robotControl.setPositionAngle(-1.0f, 1.0f,  -PI/2);
     while(!(s.millis >= 30000)) {
         state.savePlotFile(s);
-         pc.printf("rho: %f, gamma: %f, delta: %f\n", robotControl.getDistanceError(), robotControl.getThetaErrorToGoal()  * 180/PI, robotControl.getThetaGoal()*180/PI);
+    };
+
+    robotControl.setPositionAngle(-1.0f, 0.0f,  0.0f);
+    while(!(s.millis >= 45000)) {
+        state.savePlotFile(s);
     };
-            state.savePlotFile(s);
+
+    robotControl.setPositionAngle(0.0f, 1.0f, PI/2);
+    while(!(s.millis >= 63000)) {
+        state.savePlotFile(s);
+    };
+
+    state.savePlotFile(s);
     state.closePlotFile();
     state.stop();
     robotControl.setEnable(false);
-    pc.printf("\n");
-
 }