Revision 8:696c2f9dfc62, committed 2013-03-30

Comitter:

chrigelburri

Date:

Sat Mar 30 06:55:08 2013 +0000

Parent:

7:34be8b3a979c

Child:

9:d3cdcdef9719

Commit message:

vor dem tuning. nicht merh 50% duty cicle und 1.65V mitte

Changed in this revision

--- a/Actuators/MaxonESCON.cpp	Tue Mar 26 08:29:43 2013 +0000
+++ b/Actuators/MaxonESCON.cpp	Sat Mar 30 06:55:08 2013 +0000
@@ -36,17 +36,17 @@
 {
     speed = speed / ESCON_SET_FACTOR * 60.0f;
     if(speed > 1 ) {
-        _pwm = 0.9f;
+        _pwm = 0.89f;
     } else if(speed < -1) {
-        _pwm = 0.1f;
+        _pwm = 0.11f;
     } else {
-        _pwm = 0.4f*speed + 0.5f;
+        _pwm = 0.4f*speed + (0.5f * (SET_SPEED_PATCH));
     }
 }
 
 float MaxonESCON::getActualSpeed(void)
 {
-    return (_actualSpeed.read()* 2.0f - 1.0f) * ESCON_GET_FACTOR / 60.0f;
+    return (_actualSpeed.read()* 2.0f - 1.0f *(GET_SPEED_PATCH)) * ESCON_GET_FACTOR / 60.0f;
 }
 
 void MaxonESCON::period(float period)

--- a/RobotControl/RobotControl.cpp	Tue Mar 26 08:29:43 2013 +0000
+++ b/RobotControl/RobotControl.cpp	Sat Mar 30 06:55:08 2013 +0000
@@ -24,6 +24,9 @@
     motorControllerLeft->setPulses(0);
     motorControllerRight->setPulses(0);
 
+    motorControllerLeft->setVelocity(0.0f);
+    motorControllerRight->setVelocity(0.0f);
+
     Desired.setAcceleration(ACCELERATION);
     Desired.setThetaAcceleration(THETA_ACCELERATION);
 }
@@ -208,13 +211,13 @@
     stateLeft.speed = motorControllerLeft->getActualSpeed() *
                       2.0f * WHEEL_RADIUS_LEFT * PI * GEAR;
     stateRight.speed = - motorControllerRight->getActualSpeed() *
-                       2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR;
+                       2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR ;
 
     /* translational speed of the Robot (average) */
-    Actual.speed = ( stateRight.speed + stateLeft.speed ) / 2.0f;
+    Actual.speed = (stateRight.speed + stateLeft.speed) / 2.0f;
 
     /* rotational speed of the Robot  */
-    Actual.omega = ( stateRight.speed - stateLeft.speed  ) / WHEEL_DISTANCE;
+    Actual.omega = (stateRight.speed - stateLeft.speed) / WHEEL_DISTANCE;
 
     /* rotational theta of the Robot integrate the omega with the time*/
     Actual.theta += Actual.omega * period;

--- a/StateDefines/State.cpp	Tue Mar 26 08:29:43 2013 +0000
+++ b/StateDefines/State.cpp	Sat Mar 30 06:55:08 2013 +0000
@@ -46,8 +46,8 @@
                 "SetpointX-Axis[m]\t" //15
                 "SetpointY-Axis[m]\t"
                 "SetpointAngel[grad]\t" //17
-                "RightDistanceSensor[m]\t"
-                "CompassAngle[grad]\t"
+                "SetpointVelocitiy[m/s]\t"
+                "SetpointVelocitiyRotations[rad/s]\t"
                 "CompassX-Axis\t" //20
                 "CompassY-Axis\t"
                 "State\t"
@@ -81,8 +81,8 @@
             s.setxAxis,
             s.setyAxis,
             s.setAngle,
-            s.rightDist,
-            s.compassAngle,
+            s.setVelocity,
+            s.setOmega,
             s.compassxAxis,
             s.compassyAxis,
             s.state,
@@ -151,9 +151,9 @@
     s->leftPulses = - motorControllerLeft->getPulses();
     s->rightPulses = motorControllerRight->getPulses();
     s->leftVelocity = motorControllerLeft->getActualSpeed() *
-                      2.0f * WHEEL_RADIUS_LEFT * PI;
+                      2.0f * WHEEL_RADIUS_LEFT * PI * GEAR;
     s->rightVelocity = - motorControllerRight->getActualSpeed()*
-                       2.0f * WHEEL_RADIUS_RIGHT * PI;
+                       2.0f * WHEEL_RADIUS_RIGHT * PI * GEAR;
     s->velocity = robotControl->getActualSpeed();
     s->omega =  robotControl->getActualOmega();
     s->xAxis = robotControl->getxActualPosition();
@@ -166,6 +166,8 @@
     s->setyAxis = robotControl->getDesiredyPosition();
     s->setAngle = robotControl->getDesiredTheta() * 180 / PI;
     // s->compassAngle = compass->getFilteredAngle() * 180 / PI;
+    s->setVelocity = robotControl->getDesiredSpeed();
+    s->setOmega = robotControl->getDesiredOmega();
 
     s->rho = robotControl->getDistanceError();
     s->lamda = robotControl->getThetaErrorToGoal() * 180 / PI;

--- a/StateDefines/defines.h	Tue Mar 26 08:29:43 2013 +0000
+++ b/StateDefines/defines.h	Sat Mar 30 06:55:08 2013 +0000
@@ -12,11 +12,10 @@
 #define PULSES_PER_STEP       6u            // Pulses per electrical step Hallsensor, have 6 steps
 
 // Physical Dimension of the car
-#define WHEEL_RADIUS_DIFF     0.00054        // positiv zu weit links, given in [m] 0.00059
-#define WHEEL_RADIUS_LEFT     0.03930f        // radius of the left wheel, given in [m] 0.03945f
-#define WHEEL_RADIUS_RIGHT    (WHEEL_RADIUS_LEFT + WHEEL_RADIUS_DIFF)        // radius of the left wheel, given in [m]
-#define WHEEL_DISTANCE        0.1860f       // Distance of the wheel, given in [m] 0.1865f
-
+#define WHEEL_RADIUS_DIFF     -0.00015f      // positiv zu weit rechts, given in [m]
+#define WHEEL_RADIUS_LEFT     0.040280f     // radius of the left wheel, given in [m] 0.040479f
+#define WHEEL_RADIUS_RIGHT    (WHEEL_RADIUS_LEFT - WHEEL_RADIUS_DIFF)    // radius of the left wheel, given in [m]
+#define WHEEL_DISTANCE        0.1875f       // Distance of the wheel, given in [m] Grösser --> mehr drehen
 
 // State Bits of the car
 #define STATE_STOP            1u            // Bit0 = stop pressed 
@@ -25,8 +24,12 @@
 #define STATE_RIGHT           8u            // Bit3 = right ESCON in error state     
 
 // ESCON Constands
-#define ESCON_SET_FACTOR      1200.0f       // Speed Factor how set in the ESCON
-#define ESCON_GET_FACTOR      1400.0f       // Speed Factor how get in the ESCON       
+#define ESCON_SET_FACTOR      1200.0f       // Speed Factor how set in the ESCON 
+#define ESCON_GET_FACTOR      1400.0f       // Speed Factor how get in the ESCON 
+
+//Error patch of the drift of Analog input and pwn output
+#define SET_SPEED_PATCH       (1+0.0029f)  // patch factor of set speed
+#define GET_SPEED_PATCH       (1+0.0019f)  // patch factor of get speed              
 
 // Start Defintition
 #define START_X_OFFSET        -0.8f         // Sets the start X-point [m]
@@ -41,7 +44,7 @@
 #define K1                    1.0f * GAIN
 #define K2                    3.0f * GAIN
 #define K3                    2.0f * GAIN   // deafult 2.0f
-#define MIN_DISTANCE_ERROR    -0.02f         // 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.005f         // 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 * 6.555 = 21.6333333f
@@ -55,13 +58,6 @@
 
 // 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
-#define COMP_Y_MAX            238.751282f   // Maximum Y-Range
-#define COMP_Z_MAX            -266.899994f  // Maximum Z-Range not used in this side
-#define COMP_X_MIN            -90.412109f   // Minimum X-Range
-#define COMP_Y_MIN            -220.834808f  // Minimum Y-Range
-#define COMP_Z_MIN            -356.000000f  // Minimun Z-Range not used in this side
-*/
 #define COMP_X_MAX            391.219910f   // Maximum X-Range
 #define COMP_Y_MAX            382.915161f   // Maximum Y-Range      
 #define COMP_Z_MAX            -237.855042f  // Maximum Z-Range not used in this side      
@@ -112,10 +108,10 @@
     float setyAxis;
     /** Setpoint Angel [°] */
     float setAngle;
-    /** Right Distance Sensor [m] */
-    float rightDist;
-    /** Compass Angle [°] */
-    float compassAngle;
+    /** Setpoint velocitiy [m/s] */
+    float setVelocity;
+    /** Setpoint rotation velocitiy [rad/s] */
+    float setOmega;
     /** Compass X-Axis */
     float compassxAxis;
     /** Compass Y-Axis */

--- a/main.cpp	Tue Mar 26 08:29:43 2013 +0000
+++ b/main.cpp	Sat Mar 30 06:55:08 2013 +0000
@@ -1,6 +1,6 @@
 /**
  * \mainpage Index Page
- * 
+ *
  * @file main.cpp
  * @author Christian Burri
  *
@@ -91,140 +91,110 @@
     //  compass.start();
 
     state.initPlotFile();
+    state.startTimerFromZero();
+    state.start();
 
-    robotControl.start();
     robotControl.setEnable(false);
     wait(0.1);
     robotControl.setEnable(true);
     wait(0.1);
     robotControl.setAllToZero(0, 0, PI/2 );
-  //  robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
+    robotControl.start();
+    //  robotControl.setAllToZero(START_X_OFFSET, START_Y_OFFSET, PI/2 );
+
+
+    // robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
+    //  robotControl.setDesiredPositionAndAngle(0, 0, PI/2);
+    // wait(0.1);
+
+    //////////////////////////////////////////
 
-    leftMotor.setPulses(0);
-    rightMotor.setPulses(0);
+    robotControl.setDesiredPositionAndAngle(0.0f, 1.0f,  PI);
+    while(!(robotControl.getDistanceError() <= 0.1)) {
+        state.savePlotFile(s);
+    };
 
-    state.startTimerFromZero();
-    state.start();
+    robotControl.setDesiredPositionAndAngle(-1.00f, 1.0f,  -PI/2);
+    while(!(robotControl.getDistanceError() <= 0.1)) {
+        state.savePlotFile(s);
+    };
 
-   // robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
-  //  robotControl.setDesiredPositionAndAngle(0, 0, PI/2);
-   // wait(0.1);
-    
-    //////////////////////////////////////////
+    robotControl.setDesiredPositionAndAngle(-1.0f, 0.0f,  0);
+    while(!(robotControl.getDistanceError() <= 0.1)) {
+        state.savePlotFile(s);
+    };
+
+    robotControl.setDesiredPositionAndAngle(0.0f, -0.8f,  PI/2);
+    while(!(robotControl.getDistanceError() <= 0.05)) {
+        state.savePlotFile(s);
+    };
+
+    robotControl.setDesiredPositionAndAngle(0.0f, -0.2f,  PI/2);
+    while(!(robotControl.getDistanceError() <= 0.05)) {
+        state.savePlotFile(s);
+    };
+
+    robotControl.setDesiredPositionAndAngle(0.0, 0.0f,  PI/2);
+    while(!(s.millis >= 65000)) {
+        state.savePlotFile(s);
+    };
+
+
+
+
+
+    ///////////////////////stay
     /*
-            robotControl.setDesiredPositionAndAngle(0.0f, 1.0f,  PI);
-        while(!(robotControl.getDistanceError() <= 0.1)) {
+            robotControl.setDesiredPositionAndAngle(0.0, 0.0f,  PI/2);
+        while(!(s.millis >= 25000)) {
             state.savePlotFile(s);
         };
+        */
+    //////////////////////////stay
 
-        robotControl.setDesiredPositionAndAngle(-1.00f, 1.0f,  -PI/2);
-        while(!(robotControl.getDistanceError() <= 0.1)) {
-            state.savePlotFile(s);
-        };
+
+
+
+
+
+
+    ////////////////////////////////////////////////////////////////////////
+
+
 
-        robotControl.setDesiredPositionAndAngle(-1.0f, 0.0f,  0);
-        while(!(robotControl.getDistanceError() <= 0.1)) {
-            state.savePlotFile(s);
-        };
-        
-                robotControl.setDesiredPositionAndAngle(0.0f, -0.8f,  PI/2);
-        while(!(robotControl.getDistanceError() <= 0.05)) {
-            state.savePlotFile(s);
-        };
-        
-                        robotControl.setDesiredPositionAndAngle(0.0f, -0.2f,  PI/2);
+    //Zum Umfang einstellen 2m fahren
+    /*
+         robotControl.setDesiredPositionAndAngle(0.0f, 4.0f,  PI/2);
+         while(!(s.millis >= 30000)) {
+             state.savePlotFile(s);
+         };
+     */
+
+
+
+    ///////////////oder//////////////////e
+
+
+    // Zum radabstand einstellen
+
+    /*
+        robotControl.setDesiredPositionAndAngle(-0.7f, 1.0f,  PI);
         while(!(robotControl.getDistanceError() <= 0.05)) {
             state.savePlotFile(s);
         };
 
-        robotControl.setDesiredPositionAndAngle(0.0, 0.0f,  PI/2);
-        while(!(s.millis >= 65000)) {
-            state.savePlotFile(s);
-        };
-  */  
-    
-    ///////////////////////stay
-    
-            robotControl.setDesiredPositionAndAngle(0.0, 0.0f,  PI/2);
-        while(!(s.millis >= 65000)) {
+        robotControl.setDesiredPositionAndAngle(-0.8f, 1.0f,  PI);
+        while(!(robotControl.getDistanceError() <= 0.05)) {
             state.savePlotFile(s);
         };
-        
-   //////////////////////////stay
-    
-    
-    
-    
-    
-    
-    
-    
-    
-    
-    ////////////////////////////////////////////////////////////////////////
-    
-    
-
-    //Zum Umfang einstellen 2m fahren
-    
-    robotControl.setDesiredPositionAndAngle(0.0f, 2.0f,  PI/2);
-    while(!(s.millis >= 30000)) {
-        state.savePlotFile(s);
-    };
-    
-
-
-
-    ///////////////oder//////////////////
 
 
-    // Zum radabstand einstellen
-    
-    /*
-        robotControl.setDesiredPositionAndAngle(-0.94f, 0.68f,  PI);
-    while(!(s.millis >= 30000)) {
-        state.savePlotFile(s);
-    }
-   /* 
-      
-  /*  
-    int sek = 0;
-    int step = 5000;
-    int i = 0;
-    int totalTurns = 2;
-
-   sek += step;
-
-    while(i <= totalTurns) {
-        robotControl.setDesiredPositionAndAngle(0.0f, 0.0f,  PI);
-        while(!(s.millis >= sek)) {
+        robotControl.setDesiredPositionAndAngle(-1.0f, 1.0f,  PI);
+        while(!(s.millis >= 30000)) {
             state.savePlotFile(s);
-        };
-        sek += step;
+        }
+    */
 
-        robotControl.setDesiredPositionAndAngle(0.0f, 0.0f,  -PI/2);
-        while(!(s.millis >= sek)) {
-            state.savePlotFile(s);
-        };
-        sek += step;
-
-        robotControl.setDesiredPositionAndAngle(0.0f, 0.0f,  0);
-        while(!(s.millis >= sek)) {
-            state.savePlotFile(s);
-        };
-        sek += step;
-
-        robotControl.setDesiredPositionAndAngle(0.0f, 0.0f,  PI/2);
-        while(!(s.millis >= sek)) {
-            state.savePlotFile(s);
-        };
-        sek += step;
-
-        i++;
-    }
-    
-    
-*/
 
 
 ////////////////////////////////////////////////////////
@@ -232,51 +202,51 @@
 
 
     //  Epä Parkour fahrä
-/*
-        robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
-        wait(0.1);
+    /*
+            robotControl.setDesiredPositionAndAngle(START_X_OFFSET, START_Y_OFFSET, PI/2);
+            wait(0.1);
 
-        robotControl.setDesiredPositionAndAngle(-1.20f, 1.50f,  3*PI/4);
-        while(!(robotControl.getDistanceError() <= 0.4)) {
-            state.savePlotFile(s);
-        };
+            robotControl.setDesiredPositionAndAngle(-1.20f, 1.50f,  3*PI/4);
+            while(!(robotControl.getDistanceError() <= 0.4)) {
+                state.savePlotFile(s);
+            };
 
-        robotControl.setDesiredPositionAndAngle(-1.20f, 2.5f,  PI/4);
-        while(!(robotControl.getDistanceError() <= 0.4)) {
-            state.savePlotFile(s);
-        };
+            robotControl.setDesiredPositionAndAngle(-1.20f, 2.5f,  PI/4);
+            while(!(robotControl.getDistanceError() <= 0.4)) {
+                state.savePlotFile(s);
+            };
 
-        robotControl.setDesiredPositionAndAngle(-0.45f, 3.2f,  3*PI/4);
-        while(!(robotControl.getDistanceError() <= 0.4)) {
-            state.savePlotFile(s);
-        };
+            robotControl.setDesiredPositionAndAngle(-0.45f, 3.2f,  3*PI/4);
+            while(!(robotControl.getDistanceError() <= 0.4)) {
+                state.savePlotFile(s);
+            };
 
-        robotControl.setDesiredPositionAndAngle(-1.0f, 3.6f,  PI);
-        while(!(robotControl.getDistanceError() <= 0.2)) {
-            state.savePlotFile(s);
-        };
+            robotControl.setDesiredPositionAndAngle(-1.0f, 3.6f,  PI);
+            while(!(robotControl.getDistanceError() <= 0.2)) {
+                state.savePlotFile(s);
+            };
 
-        robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f,  PI);
-        while(!(robotControl.getDistanceError() <= 0.1)) {
-            state.savePlotFile(s);
-        };
+            robotControl.setDesiredPositionAndAngle(-1.5f, 3.6f,  PI);
+            while(!(robotControl.getDistanceError() <= 0.1)) {
+                state.savePlotFile(s);
+            };
 
-        robotControl.setDesiredPositionAndAngle(-2.5f, 3.0f,  -PI/2);
-        while(!(robotControl.getDistanceError() <= 0.4)) {
-            state.savePlotFile(s);
-        };
+            robotControl.setDesiredPositionAndAngle(-2.5f, 3.0f,  -PI/2);
+            while(!(robotControl.getDistanceError() <= 0.4)) {
+                state.savePlotFile(s);
+            };
 
-        robotControl.setDesiredPositionAndAngle(-1.75f, 1.30f,  -PI/2);
-        while(!(robotControl.getDistanceError() <= 0.06)) {
-            state.savePlotFile(s);
-        };
-        
+            robotControl.setDesiredPositionAndAngle(-1.75f, 1.30f,  -PI/2);
+            while(!(robotControl.getDistanceError() <= 0.06)) {
+                state.savePlotFile(s);
+            };
+
 
-    */
-    
-    
-    
-    
+        */
+
+
+
+
     /*
         printf("here we go... \n");
         AdkTerm.setupDevice();