This program is for an autonomous robot for the competition at the Hochschule Luzern. http://cruisingcrepe.wordpress.com/ We are one of the 32 teams. http://cruisingcrepe.wordpress.com/ 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: http://www.dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf
Fork of autonomous Robot Android by
StateDefines/defines.h@10:09ddb819fdcb, 2013-04-04 (annotated)
- Committer:
- chrigelburri
- Date:
- Thu Apr 04 06:43:43 2013 +0000
- Revision:
- 10:09ddb819fdcb
- Parent:
- 9:d3cdcdef9719
- Child:
- 11:775ebb69d5e1
vor dem sauberen kommentieren
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
chrigelburri | 0:31f7be68e52d | 1 | #ifndef _DEFINES_H_ |
chrigelburri | 0:31f7be68e52d | 2 | #define _DEFINES_H_ |
chrigelburri | 0:31f7be68e52d | 3 | |
chrigelburri | 10:09ddb819fdcb | 4 | /*! \file defines.h |
chrigelburri | 10:09ddb819fdcb | 5 | \brief All defines for the roboter you can see here. |
chrigelburri | 10:09ddb819fdcb | 6 | */ |
chrigelburri | 10:09ddb819fdcb | 7 | |
chrigelburri | 0:31f7be68e52d | 8 | #include "mbed.h" |
chrigelburri | 0:31f7be68e52d | 9 | |
chrigelburri | 10:09ddb819fdcb | 10 | /** |
chrigelburri | 10:09ddb819fdcb | 11 | * Physical dimensions π |
chrigelburri | 10:09ddb819fdcb | 12 | */ |
chrigelburri | 10:09ddb819fdcb | 13 | #define PI 3.141592654f |
chrigelburri | 10:09ddb819fdcb | 14 | |
chrigelburri | 10:09ddb819fdcb | 15 | /** |
chrigelburri | 10:09ddb819fdcb | 16 | * @name maxon motor #339282 EC 45 flat 30W |
chrigelburri | 10:09ddb819fdcb | 17 | * @{ |
chrigelburri | 10:09ddb819fdcb | 18 | */ |
chrigelburri | 10:09ddb819fdcb | 19 | |
chrigelburri | 10:09ddb819fdcb | 20 | /** |
chrigelburri | 10:09ddb819fdcb | 21 | * Number of of pole pairs |
chrigelburri | 10:09ddb819fdcb | 22 | */ |
chrigelburri | 10:09ddb819fdcb | 23 | #define POLE_PAIRS 8u |
chrigelburri | 0:31f7be68e52d | 24 | |
chrigelburri | 10:09ddb819fdcb | 25 | /** |
chrigelburri | 10:09ddb819fdcb | 26 | * Gear on the motor 1/11.6f |
chrigelburri | 10:09ddb819fdcb | 27 | */ |
chrigelburri | 10:09ddb819fdcb | 28 | #define GEAR 1/11.6f |
chrigelburri | 10:09ddb819fdcb | 29 | |
chrigelburri | 10:09ddb819fdcb | 30 | /** |
chrigelburri | 10:09ddb819fdcb | 31 | * Pulses per electrical step form the Hallsensor, have 6 steps |
chrigelburri | 10:09ddb819fdcb | 32 | */ |
chrigelburri | 10:09ddb819fdcb | 33 | #define PULSES_PER_STEP 6u |
chrigelburri | 10:09ddb819fdcb | 34 | /*! @} */ |
chrigelburri | 10:09ddb819fdcb | 35 | |
chrigelburri | 10:09ddb819fdcb | 36 | /** |
chrigelburri | 10:09ddb819fdcb | 37 | * @name Physical Dimension of the car |
chrigelburri | 10:09ddb819fdcb | 38 | * @{ |
chrigelburri | 10:09ddb819fdcb | 39 | */ |
chrigelburri | 10:09ddb819fdcb | 40 | |
chrigelburri | 10:09ddb819fdcb | 41 | /** |
chrigelburri | 10:09ddb819fdcb | 42 | * Value for the diffrerenz between left an right, given in [m] |
chrigelburri | 10:09ddb819fdcb | 43 | */ |
chrigelburri | 10:09ddb819fdcb | 44 | #define WHEEL_RADIUS_DIFF 0.0000f |
chrigelburri | 0:31f7be68e52d | 45 | |
chrigelburri | 10:09ddb819fdcb | 46 | /** |
chrigelburri | 10:09ddb819fdcb | 47 | * radius of the left wheel, given in [m] |
chrigelburri | 10:09ddb819fdcb | 48 | */ |
chrigelburri | 10:09ddb819fdcb | 49 | #define WHEEL_RADIUS_LEFT 0.040280f |
chrigelburri | 10:09ddb819fdcb | 50 | |
chrigelburri | 10:09ddb819fdcb | 51 | /** |
chrigelburri | 10:09ddb819fdcb | 52 | * radius of the left wheel, given in [m] |
chrigelburri | 10:09ddb819fdcb | 53 | */ |
chrigelburri | 10:09ddb819fdcb | 54 | #define WHEEL_RADIUS_RIGHT (WHEEL_RADIUS_LEFT - WHEEL_RADIUS_DIFF) |
chrigelburri | 10:09ddb819fdcb | 55 | |
chrigelburri | 10:09ddb819fdcb | 56 | /** |
chrigelburri | 10:09ddb819fdcb | 57 | * Distance of the wheel, given in [m] Greater --> turn more |
chrigelburri | 10:09ddb819fdcb | 58 | */ |
chrigelburri | 10:09ddb819fdcb | 59 | #define WHEEL_DISTANCE 0.2000f |
chrigelburri | 10:09ddb819fdcb | 60 | /*! @} */ |
chrigelburri | 10:09ddb819fdcb | 61 | |
chrigelburri | 10:09ddb819fdcb | 62 | /** |
chrigelburri | 10:09ddb819fdcb | 63 | * @name State Bits of the car |
chrigelburri | 10:09ddb819fdcb | 64 | * @{ |
chrigelburri | 10:09ddb819fdcb | 65 | */ |
chrigelburri | 0:31f7be68e52d | 66 | |
chrigelburri | 10:09ddb819fdcb | 67 | /** |
chrigelburri | 10:09ddb819fdcb | 68 | * Bit0 = stop pressed |
chrigelburri | 10:09ddb819fdcb | 69 | */ |
chrigelburri | 10:09ddb819fdcb | 70 | #define STATE_STOP 1u |
chrigelburri | 10:09ddb819fdcb | 71 | |
chrigelburri | 10:09ddb819fdcb | 72 | /** |
chrigelburri | 10:09ddb819fdcb | 73 | * Bit1 = Undervoltage battery |
chrigelburri | 10:09ddb819fdcb | 74 | */ |
chrigelburri | 10:09ddb819fdcb | 75 | #define STATE_UNDER 2u |
chrigelburri | 10:09ddb819fdcb | 76 | |
chrigelburri | 10:09ddb819fdcb | 77 | /** |
chrigelburri | 10:09ddb819fdcb | 78 | * Bit2 = left ESCON in error state |
chrigelburri | 10:09ddb819fdcb | 79 | */ |
chrigelburri | 10:09ddb819fdcb | 80 | #define STATE_LEFT 4u |
chrigelburri | 10:09ddb819fdcb | 81 | |
chrigelburri | 10:09ddb819fdcb | 82 | /** |
chrigelburri | 10:09ddb819fdcb | 83 | * Bit3 = right ESCON in error state |
chrigelburri | 10:09ddb819fdcb | 84 | */ |
chrigelburri | 10:09ddb819fdcb | 85 | #define STATE_RIGHT 8u |
chrigelburri | 10:09ddb819fdcb | 86 | /*! @} */ |
chrigelburri | 0:31f7be68e52d | 87 | |
chrigelburri | 3:92ba0254af87 | 88 | // ESCON Constands |
chrigelburri | 9:d3cdcdef9719 | 89 | #define ESCON_SET_FACTOR 1500.0f // Speed Factor how set in the ESCON |
chrigelburri | 9:d3cdcdef9719 | 90 | #define ESCON_GET_FACTOR 1600.4f // Speed Factor how get in the ESCON |
chrigelburri | 8:696c2f9dfc62 | 91 | |
chrigelburri | 8:696c2f9dfc62 | 92 | //Error patch of the drift of Analog input and pwn output |
chrigelburri | 9:d3cdcdef9719 | 93 | #define SET_SPEED_PATCH (1+0.00262f) // patch factor of set speed |
chrigelburri | 9:d3cdcdef9719 | 94 | #define GET_SPEED_PATCH (1+0.0019f) // patch factor of get speed |
chrigelburri | 0:31f7be68e52d | 95 | |
chrigelburri | 0:31f7be68e52d | 96 | // Start Defintition |
chrigelburri | 2:d8e1613dc38b | 97 | #define START_X_OFFSET -0.8f // Sets the start X-point [m] |
chrigelburri | 0:31f7be68e52d | 98 | #define START_Y_OFFSET 0.8f // Sets the start Y-point [m] |
chrigelburri | 0:31f7be68e52d | 99 | |
chrigelburri | 0:31f7be68e52d | 100 | // Maximum Aceeleration |
chrigelburri | 0:31f7be68e52d | 101 | #define ACCELERATION 0.25f // maximum translational acceleration, given in [m/s2] |
chrigelburri | 1:6cd533a712c6 | 102 | #define THETA_ACCELERATION 1.0f // maximum rotational acceleration, given in [rad/s2] |
chrigelburri | 1:6cd533a712c6 | 103 | |
chrigelburri | 3:92ba0254af87 | 104 | // position controller |
chrigelburri | 10:09ddb819fdcb | 105 | #define GAIN 0.8f // Main Gain |
chrigelburri | 9:d3cdcdef9719 | 106 | #define K1 0.8f * GAIN |
chrigelburri | 10:09ddb819fdcb | 107 | #define K2 3.0f * GAIN // deafult 3.0f |
chrigelburri | 10:09ddb819fdcb | 108 | #define K3 2.0f * GAIN // default 2.0f |
chrigelburri | 10:09ddb819fdcb | 109 | #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] |
chrigelburri | 0:31f7be68e52d | 110 | |
chrigelburri | 0:31f7be68e52d | 111 | // LiPo Batterie |
chrigelburri | 2:d8e1613dc38b | 112 | #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 |
chrigelburri | 2:d8e1613dc38b | 113 | #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 |
chrigelburri | 2:d8e1613dc38b | 114 | |
chrigelburri | 0:31f7be68e52d | 115 | // Frequenz for the Task |
chrigelburri | 0:31f7be68e52d | 116 | #define PERIOD_COMPASS 0.050f // 20Hz Rate for Compass HMC6352 |
chrigelburri | 0:31f7be68e52d | 117 | #define PERIOD_ROBOTCONTROL 0.001f // 1kHz Rate for Robot Control |
chrigelburri | 0:31f7be68e52d | 118 | #define PERIOD_STATE 0.001f // 1kHz Rate for State Objekt |
chrigelburri | 0:31f7be68e52d | 119 | #define PERIOD_ANDROID 0.1f // 10Hz Rate for State Objekt |
chrigelburri | 0:31f7be68e52d | 120 | |
chrigelburri | 6:48eeb41188dd | 121 | // Android Buffer Size for communication |
chrigelburri | 6:48eeb41188dd | 122 | #define OUTL 100 |
chrigelburri | 10:09ddb819fdcb | 123 | #define INBL 100 |
chrigelburri | 0:31f7be68e52d | 124 | |
chrigelburri | 10:09ddb819fdcb | 125 | /** |
chrigelburri | 10:09ddb819fdcb | 126 | * struct state |
chrigelburri | 10:09ddb819fdcb | 127 | * structure containing system sensor data |
chrigelburri | 10:09ddb819fdcb | 128 | **/ |
chrigelburri | 0:31f7be68e52d | 129 | typedef struct state { |
chrigelburri | 0:31f7be68e52d | 130 | /** millis Time [ms]*/ |
chrigelburri | 0:31f7be68e52d | 131 | int millis; |
chrigelburri | 0:31f7be68e52d | 132 | /** Battery voltage [V] */ |
chrigelburri | 0:31f7be68e52d | 133 | float voltageBattery; |
chrigelburri | 0:31f7be68e52d | 134 | /** Number of pulses left */ |
chrigelburri | 0:31f7be68e52d | 135 | int leftPulses; |
chrigelburri | 0:31f7be68e52d | 136 | /** Number of pulses right */ |
chrigelburri | 0:31f7be68e52d | 137 | int rightPulses; |
chrigelburri | 0:31f7be68e52d | 138 | /** Velocity left [m/s] */ |
chrigelburri | 0:31f7be68e52d | 139 | float leftVelocity; |
chrigelburri | 0:31f7be68e52d | 140 | /** Velocity right [m/s] */ |
chrigelburri | 0:31f7be68e52d | 141 | float rightVelocity; |
chrigelburri | 0:31f7be68e52d | 142 | /** Velocity of the car [m/s] */ |
chrigelburri | 0:31f7be68e52d | 143 | float velocity; |
chrigelburri | 0:31f7be68e52d | 144 | /** Velocity rotation [°/s] */ |
chrigelburri | 0:31f7be68e52d | 145 | float omega; |
chrigelburri | 0:31f7be68e52d | 146 | /** X-Axis from co-ordinate [m] */ |
chrigelburri | 0:31f7be68e52d | 147 | float xAxis; |
chrigelburri | 0:31f7be68e52d | 148 | /** Y-Axis from co-ordinate [m] */ |
chrigelburri | 0:31f7be68e52d | 149 | float yAxis; |
chrigelburri | 0:31f7be68e52d | 150 | /** X-Axis Error [m] */ |
chrigelburri | 0:31f7be68e52d | 151 | float xAxisError; |
chrigelburri | 0:31f7be68e52d | 152 | /** X-Axis Error [m] */ |
chrigelburri | 0:31f7be68e52d | 153 | float yAxisError; |
chrigelburri | 0:31f7be68e52d | 154 | /** Angle Error [°] */ |
chrigelburri | 0:31f7be68e52d | 155 | float angleError; |
chrigelburri | 0:31f7be68e52d | 156 | /** Angle from Car [°] */ |
chrigelburri | 0:31f7be68e52d | 157 | float angle; |
chrigelburri | 0:31f7be68e52d | 158 | /** Setpoint X-Axis [m] */ |
chrigelburri | 0:31f7be68e52d | 159 | float setxAxis; |
chrigelburri | 0:31f7be68e52d | 160 | /** Setpoint Y-Axis [m] */ |
chrigelburri | 0:31f7be68e52d | 161 | float setyAxis; |
chrigelburri | 0:31f7be68e52d | 162 | /** Setpoint Angel [°] */ |
chrigelburri | 0:31f7be68e52d | 163 | float setAngle; |
chrigelburri | 8:696c2f9dfc62 | 164 | /** Setpoint velocitiy [m/s] */ |
chrigelburri | 8:696c2f9dfc62 | 165 | float setVelocity; |
chrigelburri | 8:696c2f9dfc62 | 166 | /** Setpoint rotation velocitiy [rad/s] */ |
chrigelburri | 8:696c2f9dfc62 | 167 | float setOmega; |
chrigelburri | 0:31f7be68e52d | 168 | /** Compass X-Axis */ |
chrigelburri | 0:31f7be68e52d | 169 | float compassxAxis; |
chrigelburri | 0:31f7be68e52d | 170 | /** Compass Y-Axis */ |
chrigelburri | 0:31f7be68e52d | 171 | float compassyAxis; |
chrigelburri | 0:31f7be68e52d | 172 | /** State of the Car **/ |
chrigelburri | 0:31f7be68e52d | 173 | int state; |
chrigelburri | 2:d8e1613dc38b | 174 | /** distance to Goal */ |
chrigelburri | 2:d8e1613dc38b | 175 | float rho; |
chrigelburri | 2:d8e1613dc38b | 176 | /** theta to goal */ |
chrigelburri | 2:d8e1613dc38b | 177 | float lamda; |
chrigelburri | 2:d8e1613dc38b | 178 | /** theta from the goal */ |
chrigelburri | 2:d8e1613dc38b | 179 | float delta; |
chrigelburri | 0:31f7be68e52d | 180 | } state_t; |
chrigelburri | 0:31f7be68e52d | 181 | |
chrigelburri | 0:31f7be68e52d | 182 | |
chrigelburri | 0:31f7be68e52d | 183 | #endif |